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