HelperNister5

- Split autogenerated code into two functions to fix error message on Android
lessthanoptimal · Jun 17, 2024 · a2049f8 · a2049f8
1 parent 6a4dc8d
commit a2049f8
Show file tree

Hide file tree

Showing 3 changed files with 23 additions and 9 deletions.
diff --git a/main/boofcv-geo/src/main/java/boofcv/alg/geo/f/EssentialNister5.java b/main/boofcv-geo/src/main/java/boofcv/alg/geo/f/EssentialNister5.java
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2022, Peter Abeles. All Rights Reserved.
+ * Copyright (c) 2024, Peter Abeles. All Rights Reserved.
  *
  * This file is part of BoofCV (http://boofcv.org).
  *
@@ -134,8 +134,10 @@ public boolean processPointing( List<AssociatedPair3D> points, DogArray<DMatrixR
 	private boolean solveForSolutions( DogArray<DMatrixRMaj> solutions ) {
 		// Construct a linear system based on the 10 constraint equations. See equations 5,6, and 10 .
 		helper.setNullSpace(X, Y, Z, W);
-		helper.setupA1(A1);
-		helper.setupA2(A2);
+		helper.setupA1p1(A1);
+		helper.setupA1p2(A1);
+		helper.setupA2p1(A2);
+		helper.setupA2p2(A2);
 
 		// instead of Gauss-Jordan elimination LU decomposition is used to solve the system
 		if (!solver.setA(A1))

diff --git a/main/boofcv-geo/src/main/java/boofcv/alg/geo/f/HelperNister5.java b/main/boofcv-geo/src/main/java/boofcv/alg/geo/f/HelperNister5.java
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021, Peter Abeles. All Rights Reserved.
+ * Copyright (c) 2024, Peter Abeles. All Rights Reserved.
  *
  * This file is part of BoofCV (http://boofcv.org).
  *
@@ -55,7 +55,7 @@ public void setNullSpace( double[] x, double[] y, double[] z, double[] w ) {
 	 *
 	 * @param A a 10 by 10 matrix
 	 */
-	public void setupA1( DMatrixRMaj A ) {
+	public void setupA1p1( DMatrixRMaj A ) {
 		// @formatter:off
 		A.data[0] = X20*( X01*X12 - X02*X11 ) + X10*( -X01*X22 + X02*X21 ) + X00*( X11*X22 - X12*X21 );
 		A.data[1] = Y02*( Y10*Y21 - Y11*Y20 ) + Y00*( Y11*Y22 - Y12*Y21 ) + Y01*( -Y10*Y22 + Y12*Y20 );
@@ -107,6 +107,11 @@ public void setupA1( DMatrixRMaj A ) {
 		A.data[47] = Y10*( W00*Y00 - W01*Y01 - W02*Y02 + 1.5*W10*Y10 + W11*Y11 + W12*Y12 + W20*Y20 - W21*Y21 - W22*Y22 ) + 0.5*( W10*( Y00*Y00 - Y01*Y01 - Y02*Y02 + Y11*Y11 + Y12*Y12 + Y20*Y20 - Y21*Y21 - Y22*Y22 ) ) + Y00*( W01*Y11 + W02*Y12 + W11*Y01 + W12*Y02 ) + Y20*( W11*Y21 + W12*Y22 + W21*Y11 + W22*Y12 ) + W00*( Y01*Y11 + Y02*Y12 ) + W20*( Y11*Y21 + Y12*Y22 );
 		A.data[48] = Z10*( X00*Y00 - X01*Y01 - X02*Y02 + 3.0*X10*Y10 + X11*Y11 + X12*Y12 + X20*Y20 - X21*Y21 - X22*Y22 ) + X10*( Y00*Z00 - Y01*Z01 - Y02*Z02 + Y11*Z11 + Y12*Z12 + Y20*Z20 - Y21*Z21 - Y22*Z22 ) + Y10*( X00*Z00 - X01*Z01 - X02*Z02 + X11*Z11 + X12*Z12 + X20*Z20 - X21*Z21 - X22*Z22 ) + Y00*( X01*Z11 + X02*Z12 + X11*Z01 + X12*Z02 ) + Z20*( X11*Y21 + X12*Y22 + X21*Y11 + X22*Y12 ) + Z00*( X01*Y11 + X02*Y12 + X11*Y01 + X12*Y02 ) + Y20*( X11*Z21 + X12*Z22 + X21*Z11 + X22*Z12 ) + X00*( Y01*Z11 + Y02*Z12 + Y11*Z01 + Y12*Z02 ) + X20*( Y11*Z21 + Y12*Z22 + Y21*Z11 + Y22*Z12 );
 		A.data[49] = X10*( W00*Y00 - W01*Y01 - W02*Y02 + 3.0*W10*Y10 + W11*Y11 + W12*Y12 + W20*Y20 - W21*Y21 - W22*Y22 ) + Y10*( W00*X00 - W01*X01 - W02*X02 + W11*X11 + W12*X12 + W20*X20 - W21*X21 - W22*X22 ) + W10*( X00*Y00 - X01*Y01 - X02*Y02 + X11*Y11 + X12*Y12 + X20*Y20 - X21*Y21 - X22*Y22 ) + Y00*( W01*X11 + W02*X12 + W11*X01 + W12*X02 ) + W20*( X11*Y21 + X12*Y22 + X21*Y11 + X22*Y12 ) + Y20*( W11*X21 + W12*X22 + W21*X11 + W22*X12 ) + W00*( X01*Y11 + X02*Y12 + X11*Y01 + X12*Y02 ) + X00*( W01*Y11 + W02*Y12 + W11*Y01 + W12*Y02 ) + X20*( W11*Y21 + W12*Y22 + W21*Y11 + W22*Y12 );
+		// @formatter:on
+	}
+
+	public void setupA1p2( DMatrixRMaj A ) {
+		// @formatter:off
 		A.data[50] = X11*( -0.5*X00*X00 + 0.5*X01*X01 - 0.5*X02*X02 + 0.5*X10*X10 + 0.5*X11*X11 + 0.5*X12*X12 - 0.5*X20*X20 + 0.5*X21*X21 - 0.5*X22*X22 ) + X10*( X00*X01 + X20*X21 ) + X12*( X01*X02 + X21*X22 );
 		A.data[51] = Y11*( -0.5*Y00*Y00 + 0.5*Y01*Y01 - 0.5*Y02*Y02 + 0.5*Y10*Y10 + 0.5*Y11*Y11 + 0.5*Y12*Y12 - 0.5*Y20*Y20 + 0.5*Y21*Y21 - 0.5*Y22*Y22 ) + Y01*( Y00*Y10 + Y02*Y12 ) + Y21*( Y10*Y20 + Y12*Y22 );
 		A.data[52] = X11*( -X00*Y00 + X01*Y01 - X02*Y02 + X10*Y10 + 1.5*X11*Y11 + X12*Y12 - X20*Y20 + X21*Y21 - X22*Y22 ) + 0.5*( Y11*( -X00*X00 + X01*X01 - X02*X02 + X10*X10 + X12*X12 - X20*X20 + X21*X21 - X22*X22 ) ) + X12*( X01*Y02 + X02*Y01 + X21*Y22 + X22*Y21 ) + X10*( X00*Y01 + X01*Y00 + X20*Y21 + X21*Y20 ) + Y10*( X00*X01 + X20*X21 ) + Y12*( X01*X02 + X21*X22 );
@@ -167,7 +172,7 @@ public void setupA1( DMatrixRMaj A ) {
 	 *
 	 * @param B a 10 by 10 matrix
 	 */
-	public void setupA2( DMatrixRMaj B ) {
+	public void setupA2p1( DMatrixRMaj B ) {
 		// @formatter:off
 		B.data[0] = Z22*( X00*Z11 - X01*Z10 - X10*Z01 + X11*Z00 ) + Z21*( -X00*Z12 + X02*Z10 + X10*Z02 - X12*Z00 ) + Z20*( X01*Z12 - X02*Z11 - X11*Z02 + X12*Z01 ) + Z01*( X20*Z12 - X22*Z10 ) + Z00*( -X21*Z12 + X22*Z11 ) + Z02*( -X20*Z11 + X21*Z10 );
 		B.data[1] = Z22*( W00*X11 - W01*X10 - W10*X01 + W11*X00 ) + Z21*( -W00*X12 + W02*X10 + W10*X02 - W12*X00 ) + Z20*( W01*X12 - W02*X11 - W11*X02 + W12*X01 ) + Z01*( -W10*X22 + W12*X20 + W20*X12 - W22*X10 ) + Z00*( W11*X22 - W12*X21 - W21*X12 + W22*X11 ) + Z02*( W10*X21 - W11*X20 - W20*X11 + W21*X10 ) + Z12*( -W00*X21 + W01*X20 + W20*X01 - W21*X00 ) + Z11*( W00*X22 - W02*X20 - W20*X02 + W22*X00 ) + Z10*( -W01*X22 + W02*X21 + W21*X02 - W22*X01 );
@@ -219,6 +224,11 @@ public void setupA2( DMatrixRMaj B ) {
 		B.data[47] = Z10*( W00*Z00 - W01*Z01 - W02*Z02 + 1.5*W10*Z10 + W11*Z11 + W12*Z12 + W20*Z20 - W21*Z21 - W22*Z22 ) + 0.5*( W10*( Z00*Z00 - Z01*Z01 - Z02*Z02 + Z11*Z11 + Z12*Z12 + Z20*Z20 - Z21*Z21 - Z22*Z22 ) ) + Z20*( W11*Z21 + W12*Z22 + W21*Z11 + W22*Z12 ) + Z00*( W01*Z11 + W02*Z12 + W11*Z01 + W12*Z02 ) + W00*( Z01*Z11 + Z02*Z12 ) + W20*( Z11*Z21 + Z12*Z22 );
 		B.data[48] = W10*( W00*Z00 - W01*Z01 - W02*Z02 + 1.5*W10*Z10 + W11*Z11 + W12*Z12 + W20*Z20 - W21*Z21 - W22*Z22 ) + 0.5*( Z10*( W00*W00 - W01*W01 - W02*W02 + W11*W11 + W12*W12 + W20*W20 - W21*W21 - W22*W22 ) ) + W00*( W01*Z11 + W02*Z12 + W11*Z01 + W12*Z02 ) + W20*( W11*Z21 + W12*Z22 + W21*Z11 + W22*Z12 ) + Z20*( W11*W21 + W12*W22 ) + Z00*( W01*W11 + W02*W12 );
 		B.data[49] = W10*( 0.5*W00*W00 - 0.5*W01*W01 - 0.5*W02*W02 + 0.5*W10*W10 + 0.5*W11*W11 + 0.5*W12*W12 + 0.5*W20*W20 - 0.5*W21*W21 - 0.5*W22*W22 ) + W00*( W01*W11 + W02*W12 ) + W20*( W11*W21 + W12*W22 );
+		// @formatter:on
+	}
+
+	public void setupA2p2( DMatrixRMaj B ) {
+		// @formatter:off
 		B.data[50] = Z11*( -X00*Z00 + X01*Z01 - X02*Z02 + X10*Z10 + 1.5*X11*Z11 + X12*Z12 - X20*Z20 + X21*Z21 - X22*Z22 ) + 0.5*( X11*( -Z00*Z00 + Z01*Z01 - Z02*Z02 + Z10*Z10 + Z12*Z12 - Z20*Z20 + Z21*Z21 - Z22*Z22 ) ) + Z21*( X10*Z20 + X12*Z22 + X20*Z10 + X22*Z12 ) + Z01*( X00*Z10 + X02*Z12 + X10*Z00 + X12*Z02 ) + Z12*( X01*Z02 + X21*Z22 ) + Z10*( X01*Z00 + X21*Z20 );
 		B.data[51] = Z11*( -W00*X00 + W01*X01 - W02*X02 + W10*X10 + 3.0*W11*X11 + W12*X12 - W20*X20 + W21*X21 - W22*X22 ) + X11*( -W00*Z00 + W01*Z01 - W02*Z02 + W10*Z10 + W12*Z12 - W20*Z20 + W21*Z21 - W22*Z22 ) + W11*( -X00*Z00 + X01*Z01 - X02*Z02 + X10*Z10 + X12*Z12 - X20*Z20 + X21*Z21 - X22*Z22 ) + Z21*( W10*X20 + W12*X22 + W20*X10 + W22*X12 ) + Z01*( W00*X10 + W02*X12 + W10*X00 + W12*X02 ) + W21*( X10*Z20 + X12*Z22 + X20*Z10 + X22*Z12 ) + W01*( X00*Z10 + X02*Z12 + X10*Z00 + X12*Z02 ) + X01*( W00*Z10 + W02*Z12 + W10*Z00 + W12*Z02 ) + X21*( W10*Z20 + W12*Z22 + W20*Z10 + W22*Z12 );
 		B.data[52] = W11*( -W00*X00 + W01*X01 - W02*X02 + W10*X10 + 1.5*W11*X11 + W12*X12 - W20*X20 + W21*X21 - W22*X22 ) + 0.5*( X11*( -W00*W00 + W01*W01 - W02*W02 + W10*W10 + W12*W12 - W20*W20 + W21*W21 - W22*W22 ) ) + W01*( W00*X10 + W02*X12 + W10*X00 + W12*X02 ) + W21*( W10*X20 + W12*X22 + W20*X10 + W22*X12 ) + W12*( W02*X01 + W22*X21 ) + W10*( W00*X01 + W20*X21 );

diff --git a/main/boofcv-geo/src/test/java/boofcv/alg/geo/f/TestHelperNister5.java b/main/boofcv-geo/src/test/java/boofcv/alg/geo/f/TestHelperNister5.java
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2023, Peter Abeles. All Rights Reserved.
+ * Copyright (c) 2024, Peter Abeles. All Rights Reserved.
  *
  * This file is part of BoofCV (http://boofcv.org).
  *
@@ -56,8 +56,10 @@ public TestHelperNister5() {
 
 		HelperNister5 alg = new HelperNister5();
 		alg.setNullSpace(X,Y,Z,W);
-		alg.setupA1(A);
-		alg.setupA2(B);
+		alg.setupA1p1(A);
+		alg.setupA1p2(A);
+		alg.setupA2p1(B);
+		alg.setupA2p2(B);
 
 		DMatrixRMaj Y1 = new DMatrixRMaj(10,1);
 		DMatrixRMaj Y2 = new DMatrixRMaj(10,1);