From 55d3e06e82bf3eb0c1e2d9c64fe2ef8bf8584020 Mon Sep 17 00:00:00 2001
From: Peter Abeles <peter.abeles@gmail.com>
Date: Fri, 8 Dec 2023 11:09:51 -0800
Subject: [PATCH] - InvertibleTransform   * Added concatInvert   * Se3 supports
 concatInvert natively

---
 change.txt                                    |  3 +++
 .../struct/InvertibleTransform.java           | 13 ++++++++++
 main/src/georegression/struct/se/Se3_F64.java | 24 +++++++++++++++++++
 .../GenericInvertibleTransformTests_F64.java  | 21 ++++++++++++++++
 4 files changed, 61 insertions(+)

diff --git a/change.txt b/change.txt
index 214dbdcf..5f6d3415 100644
--- a/change.txt
+++ b/change.txt
@@ -11,6 +11,9 @@ Version : 0.27.0
   * Plane and Cylinder
 - Added ShapeFittingRobustOps
   * Easy interface for using standard robust algorithms to fit points to shapes
+- InvertibleTransform
+  * Added concatInvert
+  * Se3 supports concatInvert natively
 
 ---------------------------------------------
 Date    : 2023-Nov-05
diff --git a/main/src/georegression/struct/InvertibleTransform.java b/main/src/georegression/struct/InvertibleTransform.java
index db8078b3..cfb45c27 100644
--- a/main/src/georegression/struct/InvertibleTransform.java
+++ b/main/src/georegression/struct/InvertibleTransform.java
@@ -118,6 +118,19 @@ default T invertConcat(T second, @Nullable T result) {
 		return (T)invert(null).concat(second, result);
 	}
 
+	/**
+	 * Computes a transform that's equivalent to 'this.concat(second.invert(null), result)'. The advantage of using
+	 * this function is that it might have been implemented so that the inversion is implicit, which can result in
+	 * no memory creation and more stable numerics.
+	 *
+	 * @param second The second transform which is applied. Not modified.
+	 * @param result (Output) storage for rsulting transform. Can be null
+	 * @return The computed transform. If result isn't null then result is returned.
+	 */
+	default T concatInvert(T second, @Nullable T result) {
+		return concat((T)second.invert(null), result);
+	}
+
 	/**
 	 * Sets the transform to its initial state of no transform.
 	 */
diff --git a/main/src/georegression/struct/se/Se3_F64.java b/main/src/georegression/struct/se/Se3_F64.java
index f9bc0de1..c304bd65 100644
--- a/main/src/georegression/struct/se/Se3_F64.java
+++ b/main/src/georegression/struct/se/Se3_F64.java
@@ -219,6 +219,30 @@ public Se3_F64 invertConcat(Se3_F64 second, @Nullable Se3_F64 result) {
 		return result;
 	}
 
+	/**
+	 * Implicitly inverts this transform before concating it to the second transform.
+	 * Equivalent to invert(null).concat(second, null);
+	 */
+	@Override
+	public Se3_F64 concatInvert(Se3_F64 second, @Nullable Se3_F64 result) {
+		if (result == null)
+			result = new Se3_F64();
+
+		// [R' | -R'*T]
+
+		// Compute new rotation matrix first
+		CommonOps_DDRM.multTransA(second.getR(), R, result.R);
+
+		// Store -R'*T inside of result
+		GeometryMath_F64.multTran(second.getR(), second.T, result.T);
+		result.T.scale(-1);
+
+		// Now find the translation
+		GeometryMath_F64.addMultTrans(result.T, second.R, T, result.T);
+
+		return result;
+	}
+
 	@Override
 	public Se3_F64 invert(@Nullable Se3_F64 inverse) {
 
diff --git a/main/test/georegression/struct/GenericInvertibleTransformTests_F64.java b/main/test/georegression/struct/GenericInvertibleTransformTests_F64.java
index 26e7ced4..7c287aba 100644
--- a/main/test/georegression/struct/GenericInvertibleTransformTests_F64.java
+++ b/main/test/georegression/struct/GenericInvertibleTransformTests_F64.java
@@ -133,4 +133,25 @@ public abstract class GenericInvertibleTransformTests_F64<T extends GeoTuple_F64
 			assertTrue(expected.isIdentical(found, GrlConstants.TEST_F64));
 		}
 	}
+
+	@Test void concatInvert() {
+		for (int i = 0; i < 20; i++) {
+			InvertibleTransform a = createRandomTransform();
+			InvertibleTransform b = createRandomTransform();
+
+			InvertibleTransform c = a.concat(b, null);
+
+			// Recompute B using concatInvert
+			InvertibleTransform aa = c.concatInvert(b, null);
+
+			T orig = createRandomPoint();
+
+			// These should be the same
+			T expected = apply(a, orig, null);
+			T found = apply(aa, orig, null);
+
+			// Use the norm to scale the tolerance
+			assertTrue(expected.isIdentical(found, expected.norm()*GrlConstants.TEST_F64), "trial: " + i);
+		}
+	}
 }