- InvertibleTransform

  * Added concatInvert
  * Se3 supports concatInvert natively

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

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.
 	 */

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) {
 

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);
+		}
+	}
 }