From c0eadbff13a9edc649658e53129638e588904907 Mon Sep 17 00:00:00 2001 From: Peter Abeles Date: Mon, 28 Aug 2023 07:31:45 -0700 Subject: [PATCH] AverageRotationMatrix_F64 - Added way to compute average from an arbitrary format - Cleaned up test code format --- change.txt | 2 + .../se/AverageRotationMatrix_F64.java | 132 +++++++++++----- .../se/TestAverageQuaternion_F64.java | 67 ++++---- .../se/TestAverageRotationMatrix_F64.java | 148 +++++++++--------- 4 files changed, 202 insertions(+), 147 deletions(-) diff --git a/change.txt b/change.txt index 66fa0060..7e446c6b 100644 --- a/change.txt +++ b/change.txt @@ -6,6 +6,8 @@ Version : 0.26.2 - LineSegment2D * Added pointOnLine functions +- AverageRotationMatrix + * Quality of life improvements --------------------------------------------- Date : 2023-May-15 diff --git a/main/src/georegression/transform/se/AverageRotationMatrix_F64.java b/main/src/georegression/transform/se/AverageRotationMatrix_F64.java index 6efb3587..493dcefe 100644 --- a/main/src/georegression/transform/se/AverageRotationMatrix_F64.java +++ b/main/src/georegression/transform/se/AverageRotationMatrix_F64.java @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021, Peter Abeles. All Rights Reserved. + * Copyright (C) 2022, Peter Abeles. All Rights Reserved. * * This file is part of Geometric Regression Library (GeoRegression). * @@ -41,16 +41,31 @@ */ public class AverageRotationMatrix_F64 { - DMatrixRMaj M = new DMatrixRMaj(3,3); - DMatrix3x3 F = new DMatrix3x3(); + // Internal workspace to store the average + private DMatrixRMaj M = new DMatrixRMaj(3, 3); + private DMatrix3x3 F = new DMatrix3x3(); - SingularValueDecomposition_F64 svd = DecompositionFactory_DDRM.svd(3,3,true,true,true); + // Used to store an extracted matrix + private DMatrix3x3 ftmp = new DMatrix3x3(); - public boolean process(List list , DMatrixRMaj average ) { + SingularValueDecomposition_F64 svd = DecompositionFactory_DDRM.svd(3, 3, true, true, true); - if( list.isEmpty() ) + // Storage for U,V in SVD + private DMatrixRMaj U = new DMatrixRMaj(3, 3); + private DMatrixRMaj V = new DMatrixRMaj(3, 3); + + /** + * Finds average rotation matrix for matrices stored in {@link DMatrixRMaj} format. + * + * @param list (Input) rotation matrices + * @param average (Output) average rotation. + * @return true if successful or false if it failed + */ + public boolean process( List list, DMatrixRMaj average ) { + + if (list.isEmpty()) throw new IllegalArgumentException("Input list is empty"); - if( average == null ) + if (average == null) throw new IllegalArgumentException("average is null"); M.zero(); @@ -59,67 +74,108 @@ public boolean process(List list , DMatrixRMaj average ) { DMatrixRMaj m = list.get(i); // unroll to make it faster. M = M + m - // row 0 - M.data[0] += m.data[0]; M.data[1] += m.data[1]; M.data[2] += m.data[2]; - // row 1 - M.data[3] += m.data[3]; M.data[4] += m.data[4]; M.data[5] += m.data[5]; - // row 2 - M.data[6] += m.data[6]; M.data[7] += m.data[7]; M.data[8] += m.data[8]; + //@formatter:off + M.data[0] += m.data[0]; M.data[1] += m.data[1]; M.data[2] += m.data[2]; // row 0 + M.data[3] += m.data[3]; M.data[4] += m.data[4]; M.data[5] += m.data[5]; // row 1 + M.data[6] += m.data[6]; M.data[7] += m.data[7]; M.data[8] += m.data[8]; // row 2 + //@formatter:on } - CommonOps_DDRM.divide(M,list.size()); + CommonOps_DDRM.divide(M, list.size()); - if( !svd.decompose(M) ) + if (!svd.decompose(M)) return false; - CommonOps_DDRM.multTransB(svd.getU(null,false),svd.getV(null,false),average); + CommonOps_DDRM.multTransB(svd.getU(U, false), svd.getV(V, false), average); // determinant should be +1 double det = CommonOps_DDRM.det(average); - if( det < 0 ) - CommonOps_DDRM.scale(-1,average); + if (det < 0) + CommonOps_DDRM.scale(-1, average); return true; } - public boolean process(List list , DMatrix3x3 average ) { + /** + * Finds average rotation matrix for matrices stored in {@link DMatrix3x3} format. + * + * @param list (Input) rotation matrices + * @param average (Output) average rotation. + * @return true if successful or false if it failed + */ + public boolean process( List list, DMatrix3x3 average ) { - if( list.isEmpty() ) + if (list.isEmpty()) throw new IllegalArgumentException("Input list is empty"); - if( average == null ) + if (average == null) throw new IllegalArgumentException("average is null"); - CommonOps_DDF3.fill(F,0); + CommonOps_DDF3.fill(F, 0); for (int i = 0; i < list.size(); i++) { - DMatrix3x3 m = list.get(i); - - // unroll to make it faster. M = M + m - // row 0 - F.a11 += m.a11; F.a12 += m.a12; F.a13 += m.a13; - // row 1 - F.a21 += m.a21; F.a22 += m.a22; F.a23 += m.a23; - // row 2 - F.a31 += m.a31; F.a32 += m.a32; F.a33 += m.a33; + CommonOps_DDF3.add(F, list.get(i), F); } - CommonOps_DDF3.divide(F,list.size()); + CommonOps_DDF3.divide(F, list.size()); - DConvertMatrixStruct.convert(F,M); - if( !svd.decompose(M) ) + DConvertMatrixStruct.convert(F, M); + if (!svd.decompose(M)) return false; - CommonOps_DDRM.multTransB(svd.getU(null,false),svd.getV(null,false),M); + CommonOps_DDRM.multTransB(svd.getU(U, false), svd.getV(V, false), M); // determinant should be +1 double det = CommonOps_DDRM.det(M); - if( det < 0 ) - CommonOps_DDRM.scale(-1,M); + if (det < 0) + CommonOps_DDRM.scale(-1, M); + + DConvertMatrixStruct.convert(M, average); + + return true; + } + + /** + * Computes average rotation matrix given a list with an arbitrary data type that can be converted into + * a {@link DMatrix3x3}. + * + * @param list List of data structures with rotation matrix in it + * @param average (Output) Storage for computed average matrix + * @param op Converts input data type to {@link DMatrix3x3} + * @return true if successful or false if it failed + */ + public boolean processOp( List list, DMatrixRMaj average, Convert op ) { + if (list.isEmpty()) + throw new IllegalArgumentException("Input list is empty"); + if (average == null) + throw new IllegalArgumentException("average is null"); + + CommonOps_DDF3.fill(F, 0); - DConvertMatrixStruct.convert(M,average); + for (int i = 0; i < list.size(); i++) { + op.convert(list.get(i), ftmp); + CommonOps_DDF3.add(F, ftmp, F); + } + + CommonOps_DDF3.divide(F, list.size()); + + DConvertMatrixStruct.convert(F, average); + if (!svd.decompose(average)) + return false; + + CommonOps_DDRM.multTransB(svd.getU(U, false), svd.getV(V, false), average); + + // determinant should be +1 + double det = CommonOps_DDRM.det(average); + + if (det < 0) + CommonOps_DDRM.scale(-1, average); return true; } + + public interface Convert { + void convert( T src, DMatrix3x3 dst ); + } } diff --git a/main/test/georegression/transform/se/TestAverageQuaternion_F64.java b/main/test/georegression/transform/se/TestAverageQuaternion_F64.java index 14fea2f8..67637e64 100644 --- a/main/test/georegression/transform/se/TestAverageQuaternion_F64.java +++ b/main/test/georegression/transform/se/TestAverageQuaternion_F64.java @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021, Peter Abeles. All Rights Reserved. + * Copyright (C) 2022, Peter Abeles. All Rights Reserved. * * This file is part of Geometric Regression Library (GeoRegression). * @@ -28,14 +28,10 @@ import org.junit.jupiter.api.Test; import java.util.ArrayList; -import java.util.List; import java.util.Random; import static org.junit.jupiter.api.Assertions.assertTrue; -/** - * @author Peter Abeles - */ public class TestAverageQuaternion_F64 { Random rand = new Random(234); @@ -43,75 +39,72 @@ public class TestAverageQuaternion_F64 { /** * Find the average of one quaternion. Which should be the same as the input quaternion. */ - @Test - void one() { - Quaternion_F64 q = ConvertRotation3D_F64.eulerToQuaternion(EulerType.XYZ,0.1,-0.5,1.5,null); + @Test void one() { + Quaternion_F64 q = ConvertRotation3D_F64.eulerToQuaternion(EulerType.XYZ, 0.1, -0.5, 1.5, null); - List list = new ArrayList<>(); + var list = new ArrayList(); list.add(q); - AverageQuaternion_F64 alg = new AverageQuaternion_F64(); - Quaternion_F64 found = new Quaternion_F64(); + var alg = new AverageQuaternion_F64(); + var found = new Quaternion_F64(); - assertTrue( alg.process(list,found) ); + assertTrue(alg.process(list, found)); - checkEquals(q,found, GrlConstants.TEST_F64); + checkEquals(q, found, GrlConstants.TEST_F64); } - @Test - void two_same() { - Quaternion_F64 q = ConvertRotation3D_F64.eulerToQuaternion(EulerType.XYZ,0.1,-0.5,1.5,null); + @Test void two_same() { + Quaternion_F64 q = ConvertRotation3D_F64.eulerToQuaternion(EulerType.XYZ, 0.1, -0.5, 1.5, null); - List list = new ArrayList<>(); + var list = new ArrayList(); list.add(q); list.add(q); - AverageQuaternion_F64 alg = new AverageQuaternion_F64(); - Quaternion_F64 found = new Quaternion_F64(); + var alg = new AverageQuaternion_F64(); + var found = new Quaternion_F64(); - assertTrue( alg.process(list,found) ); + assertTrue(alg.process(list, found)); - checkEquals(q,found, GrlConstants.TEST_F64); + checkEquals(q, found, GrlConstants.TEST_F64); } /** * Generate a bunch of quaternions, but noise them up on one axis and see if the result is close to the expected. */ - @Test - void noiseOnOneAxis() { + @Test void noiseOnOneAxis() { double rotX = 0.1; double rotY = -0.5; double rotZ = 1.5; - List list = new ArrayList<>(); + var list = new ArrayList(); for (int i = 0; i < 40; i++) { double noise = rand.nextGaussian()*0.03; - list.add( ConvertRotation3D_F64.eulerToQuaternion(EulerType.XYZ,rotX,rotY+noise,rotZ,null)); + list.add(ConvertRotation3D_F64.eulerToQuaternion(EulerType.XYZ, rotX, rotY + noise, rotZ, null)); } - Quaternion_F64 expected = ConvertRotation3D_F64.eulerToQuaternion(EulerType.XYZ,rotX,rotY,rotZ,null); + Quaternion_F64 expected = ConvertRotation3D_F64.eulerToQuaternion(EulerType.XYZ, rotX, rotY, rotZ, null); - AverageQuaternion_F64 alg = new AverageQuaternion_F64(); - Quaternion_F64 found = new Quaternion_F64(); + var alg = new AverageQuaternion_F64(); + var found = new Quaternion_F64(); - assertTrue( alg.process(list,found) ); + assertTrue(alg.process(list, found)); - checkEquals(expected, found, Math.pow(GrlConstants.TEST_F64,0.3)); + checkEquals(expected, found, Math.pow(GrlConstants.TEST_F64, 0.3)); } /** * Sees if two quaternions are equal up to a sign ambiguity */ - public static void checkEquals( Quaternion_F64 expected , Quaternion_F64 found , double errorTol ) { + public static void checkEquals( Quaternion_F64 expected, Quaternion_F64 found, double errorTol ) { - DMatrixRMaj E = ConvertRotation3D_F64.quaternionToMatrix(expected,null); - DMatrixRMaj F = ConvertRotation3D_F64.quaternionToMatrix(found,null); + DMatrixRMaj E = ConvertRotation3D_F64.quaternionToMatrix(expected, null); + DMatrixRMaj F = ConvertRotation3D_F64.quaternionToMatrix(found, null); - DMatrixRMaj diff = new DMatrixRMaj(3,3); - CommonOps_DDRM.multTransA(E,F,diff); + var diff = new DMatrixRMaj(3, 3); + CommonOps_DDRM.multTransA(E, F, diff); - Rodrigues_F64 error = ConvertRotation3D_F64.matrixToRodrigues(diff,null); + Rodrigues_F64 error = ConvertRotation3D_F64.matrixToRodrigues(diff, null); - assertTrue( Math.abs(error.theta) <= 10*errorTol ); + assertTrue(Math.abs(error.theta) <= 10*errorTol); } } diff --git a/main/test/georegression/transform/se/TestAverageRotationMatrix_F64.java b/main/test/georegression/transform/se/TestAverageRotationMatrix_F64.java index 75400fa9..df1425f8 100644 --- a/main/test/georegression/transform/se/TestAverageRotationMatrix_F64.java +++ b/main/test/georegression/transform/se/TestAverageRotationMatrix_F64.java @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021, Peter Abeles. All Rights Reserved. + * Copyright (C) 2022, Peter Abeles. All Rights Reserved. * * This file is part of Geometric Regression Library (GeoRegression). * @@ -29,15 +29,11 @@ import org.junit.jupiter.api.Test; import java.util.ArrayList; -import java.util.List; import java.util.Random; import static georegression.geometry.ConvertRotation3D_F64.eulerToMatrix; import static org.junit.jupiter.api.Assertions.assertTrue; -/** - * @author Peter Abeles - */ public class TestAverageRotationMatrix_F64 { Random rand = new Random(234); @@ -45,140 +41,148 @@ public class TestAverageRotationMatrix_F64 { /** * Find the average of one quaternion. Which should be the same as the input quaternion. */ - @Test - void one_M() { - DMatrixRMaj q = eulerToMatrix(EulerType.XYZ,0.1,-0.5,1.5,null); + @Test void one_M() { + DMatrixRMaj q = eulerToMatrix(EulerType.XYZ, 0.1, -0.5, 1.5, null); + + var list = new ArrayList(); + list.add(q); + + var alg = new AverageRotationMatrix_F64(); + var found = new DMatrixRMaj(3, 3); + + assertTrue(alg.process(list, found)); + + checkEquals(q, found, GrlConstants.TEST_F64); + } + + @Test void one_F() { + var q = new DMatrix3x3(); + DConvertMatrixStruct.convert(eulerToMatrix(EulerType.XYZ, 0.1, -0.5, 1.5, null), q); - List list = new ArrayList<>(); + var list = new ArrayList(); list.add(q); - AverageRotationMatrix_F64 alg = new AverageRotationMatrix_F64(); - DMatrixRMaj found = new DMatrixRMaj(3,3); + var alg = new AverageRotationMatrix_F64(); + var found = new DMatrix3x3(); - assertTrue( alg.process(list,found) ); + assertTrue(alg.process(list, found)); - checkEquals(q,found, GrlConstants.TEST_F64); + checkEquals(q, found, GrlConstants.TEST_F64); } - @Test - void one_F() { - DMatrix3x3 q = new DMatrix3x3(); - DConvertMatrixStruct.convert(eulerToMatrix(EulerType.XYZ,0.1,-0.5,1.5,null),q); + @Test void two_same_M() { + DMatrixRMaj q = eulerToMatrix(EulerType.XYZ, 0.1, -0.5, 1.5, null); - List list = new ArrayList<>(); + var list = new ArrayList(); + list.add(q); list.add(q); - AverageRotationMatrix_F64 alg = new AverageRotationMatrix_F64(); - DMatrix3x3 found = new DMatrix3x3(); + var alg = new AverageRotationMatrix_F64(); + var found = new DMatrixRMaj(3, 3); - assertTrue( alg.process(list,found) ); + assertTrue(alg.process(list, found)); - checkEquals(q,found, GrlConstants.TEST_F64); + checkEquals(q, found, GrlConstants.TEST_F64); } - @Test - void two_same_M() { - DMatrixRMaj q = eulerToMatrix(EulerType.XYZ,0.1,-0.5,1.5,null); + @Test void two_same_F() { + var q = new DMatrix3x3(); + DConvertMatrixStruct.convert(eulerToMatrix(EulerType.XYZ, 0.1, -0.5, 1.5, null), q); - List list = new ArrayList<>(); + var list = new ArrayList(); list.add(q); list.add(q); - AverageRotationMatrix_F64 alg = new AverageRotationMatrix_F64(); - DMatrixRMaj found = new DMatrixRMaj(3,3); + var alg = new AverageRotationMatrix_F64(); + var found = new DMatrix3x3(); - assertTrue( alg.process(list,found) ); + assertTrue(alg.process(list, found)); - checkEquals(q,found, GrlConstants.TEST_F64); + checkEquals(q, found, GrlConstants.TEST_F64); } - @Test - void two_same_F() { - DMatrix3x3 q = new DMatrix3x3(); - DConvertMatrixStruct.convert(eulerToMatrix(EulerType.XYZ,0.1,-0.5,1.5,null),q); + @Test void two_same_Op() { + DMatrixRMaj q = eulerToMatrix(EulerType.XYZ, 0.1, -0.5, 1.5, null); - List list = new ArrayList<>(); + var list = new ArrayList(); list.add(q); list.add(q); - AverageRotationMatrix_F64 alg = new AverageRotationMatrix_F64(); - DMatrix3x3 found = new DMatrix3x3(); + var alg = new AverageRotationMatrix_F64(); + var found = new DMatrixRMaj(3, 3); - assertTrue( alg.process(list,found) ); + assertTrue(alg.processOp(list, found, ( src, dst ) -> dst.setTo(src))); - checkEquals(q,found, GrlConstants.TEST_F64); + checkEquals(q, found, GrlConstants.TEST_F64); } /** * Generate a bunch of quaternions, but noise them up on one axis and see if the result is close to the expected. */ - @Test - void noiseOnOneAxis_M() { + @Test void noiseOnOneAxis_M() { double rotX = 0.1; double rotY = -0.5; double rotZ = 1.5; - List list = new ArrayList<>(); + var list = new ArrayList(); for (int i = 0; i < 40; i++) { - double noise = rand.nextGaussian() * 0.03; + double noise = rand.nextGaussian()*0.03; list.add(eulerToMatrix(EulerType.XYZ, rotX, rotY + noise, rotZ, null)); } DMatrixRMaj expected = eulerToMatrix(EulerType.XYZ, 0.1, -0.5, 1.5, null); - AverageRotationMatrix_F64 alg = new AverageRotationMatrix_F64(); - DMatrixRMaj found = new DMatrixRMaj(3, 3); + var alg = new AverageRotationMatrix_F64(); + var found = new DMatrixRMaj(3, 3); assertTrue(alg.process(list, found)); - checkEquals(expected, found, Math.pow(GrlConstants.TEST_F64,0.3)); + checkEquals(expected, found, Math.pow(GrlConstants.TEST_F64, 0.3)); } - @Test - void noiseOnOneAxis_F() { + @Test void noiseOnOneAxis_F() { double rotX = 0.1; double rotY = -0.5; double rotZ = 1.5; - List list = new ArrayList<>(); + var list = new ArrayList(); for (int i = 0; i < 40; i++) { - double noise = rand.nextGaussian() * 0.03; - DMatrix3x3 q = new DMatrix3x3(); - DConvertMatrixStruct.convert(eulerToMatrix(EulerType.XYZ, rotX, rotY + noise, rotZ,null),q); + double noise = rand.nextGaussian()*0.03; + var q = new DMatrix3x3(); + DConvertMatrixStruct.convert(eulerToMatrix(EulerType.XYZ, rotX, rotY + noise, rotZ, null), q); list.add(q); } - DMatrix3x3 expected = new DMatrix3x3(); - DConvertMatrixStruct.convert(eulerToMatrix(EulerType.XYZ,0.1,-0.5,1.5,null),expected); + var expected = new DMatrix3x3(); + DConvertMatrixStruct.convert(eulerToMatrix(EulerType.XYZ, 0.1, -0.5, 1.5, null), expected); - AverageRotationMatrix_F64 alg = new AverageRotationMatrix_F64(); - DMatrix3x3 found = new DMatrix3x3(); + var alg = new AverageRotationMatrix_F64(); + var found = new DMatrix3x3(); assertTrue(alg.process(list, found)); - checkEquals(expected, found, Math.pow(GrlConstants.TEST_F64,0.3)); + checkEquals(expected, found, Math.pow(GrlConstants.TEST_F64, 0.3)); } - public static void checkEquals( DMatrixRMaj expected , DMatrixRMaj found , double errorTol ) { - DMatrixRMaj diff = new DMatrixRMaj(3,3); - CommonOps_DDRM.multTransA(expected,found,diff); + public static void checkEquals( DMatrixRMaj expected, DMatrixRMaj found, double errorTol ) { + var diff = new DMatrixRMaj(3, 3); + CommonOps_DDRM.multTransA(expected, found, diff); - Rodrigues_F64 error = ConvertRotation3D_F64.matrixToRodrigues(diff,null); + Rodrigues_F64 error = ConvertRotation3D_F64.matrixToRodrigues(diff, null); - assertTrue( Math.abs(error.theta) <= errorTol ); + assertTrue(Math.abs(error.theta) <= errorTol); } - public static void checkEquals( DMatrix3x3 expected , DMatrix3x3 found , double errorTol ) { - DMatrixRMaj E = new DMatrixRMaj(3,3); - DMatrixRMaj F = new DMatrixRMaj(3,3); + public static void checkEquals( DMatrix3x3 expected, DMatrix3x3 found, double errorTol ) { + var E = new DMatrixRMaj(3, 3); + var F = new DMatrixRMaj(3, 3); - DConvertMatrixStruct.convert(expected,E); - DConvertMatrixStruct.convert(found,F); + DConvertMatrixStruct.convert(expected, E); + DConvertMatrixStruct.convert(found, F); - DMatrixRMaj diff = new DMatrixRMaj(3,3); - CommonOps_DDRM.multTransA(E,F,diff); + var diff = new DMatrixRMaj(3, 3); + CommonOps_DDRM.multTransA(E, F, diff); - Rodrigues_F64 error = ConvertRotation3D_F64.matrixToRodrigues(diff,null); + Rodrigues_F64 error = ConvertRotation3D_F64.matrixToRodrigues(diff, null); - assertTrue( Math.abs(error.theta) <= errorTol ); + assertTrue(Math.abs(error.theta) <= errorTol); } - }