001/******************************************************************************* 002 * Permission to use and distribute this software is granted under the BSD 2-Clause 003 * "Simplified" License (see http://opensource.org/licenses/BSD-2-Clause). 004 * Copyright (c) 2016-2023 Wilhelm Burger. All rights reserved. 005 * Visit https://imagingbook.com for additional details. 006 ******************************************************************************/ 007package imagingbook.calibration.zhang.util; 008 009import imagingbook.common.geometry.basic.Pnt2d; 010import imagingbook.common.math.Matrix; 011import imagingbook.common.math.exception.DivideByZeroException; 012import org.apache.commons.math3.complex.Quaternion; 013import org.apache.commons.math3.geometry.euclidean.threed.Rotation; 014import org.apache.commons.math3.linear.Array2DRowRealMatrix; 015import org.apache.commons.math3.linear.MatrixUtils; 016import org.apache.commons.math3.linear.RealMatrix; 017import org.apache.commons.math3.linear.RealVector; 018import org.apache.commons.math3.linear.SingularValueDecomposition; 019 020/** 021 * Utility math methods used for camera calibration. 022 * 023 * @author WB 024 * @version 2022/12/19 025 */ 026public abstract class MathUtil { 027 028 private MathUtil() {} 029 030 public static double[] toArray(Pnt2d p) { 031 return new double[] {p.getX(), p.getY()}; 032 } 033 034 public static Pnt2d toPnt2d(double[] xy) { 035 return Pnt2d.from(xy); 036 } 037 038 public static RealVector crossProduct3x3(RealVector A, RealVector B) { 039 final double[] a = A.toArray(); 040 final double[] b = B.toArray(); 041 final double[] c = { 042 a[1] * b[2] - b[1] * a[2], 043 a[2] * b[0] - b[2] * a[0], 044 a[0] * b[1] - b[0] * a[1] 045 }; 046 return MatrixUtils.createRealVector(c); 047 } 048 049 public static RealVector getRowPackedVector(RealMatrix A) { 050 double[][] AA = A.getData(); 051 double[] V = new double[AA.length * AA[0].length]; 052 int k = 0; 053 for (int i = 0; i < AA.length; i++) { 054 for (int j = 0; j < AA[0].length; j++) { 055 V[k++] = AA[i][j]; 056 } 057 } 058 return MatrixUtils.createRealVector(V); 059 } 060 061 public static RealMatrix fromRowPackedVector(RealVector V, int rows, int columns) { 062 double[][] AA = new double[rows][columns]; 063 double[] data = V.toArray(); 064 int k = 0; 065 for (int i = 0; i < rows; i++) { 066 for (int j = 0; j < columns; j++) { 067 AA[i][j] = data[k++]; 068 } 069 } 070 return MatrixUtils.createRealMatrix(AA); 071 } 072 073 /** 074 * Finds a nontrivial solution (x) to the homogeneous linear system A . x = 0 by singular-value decomposition. If A 075 * has more rows than columns, the system of equations is overdetermined. In this case the returned solution 076 * minimizes the residual ||A . x|| in the least-squares sense. 077 * 078 * @param A the original matrix. 079 * @return the solution vector x. 080 */ 081 public static RealVector solveHomogeneousSystem(RealMatrix A) { 082 SingularValueDecomposition svd = new SingularValueDecomposition(A); 083 RealMatrix V = svd.getV(); 084 // RealVector x = V.getColumnVector(V.getColumnDimension() - 1); 085 // return x; 086 int minIdx = Matrix.idxMin(svd.getSingularValues()); 087 return V.getColumnVector(minIdx); 088 } 089 090 /** 091 * Converts a Cartesian vector to an equivalent homogeneous vector by attaching an additional 1-element. The 092 * resulting homogeneous vector is one element longer than the specified Cartesian vector. See also 093 * {@link #toCartesian(double[])}. 094 * 095 * @param ac a Cartesian vector 096 * @return an equivalent homogeneous vector 097 */ 098 public static double[] toHomogeneous(double[] ac) { 099 double[] xh = new double[ac.length + 1]; 100 for (int i = 0; i < ac.length; i++) { 101 xh[i] = ac[i]; 102 xh[xh.length - 1] = 1; 103 } 104 return xh; 105 } 106 107 /** 108 * Converts a homogeneous vector to its equivalent Cartesian vector, which is one element shorter. See also 109 * {@link #toHomogeneous(double[])}. 110 * 111 * @param ah a homogeneous vector 112 * @return the equivalent Cartesian vector 113 * @throws DivideByZeroException if the last vector element is zero 114 */ 115 public static double[] toCartesian(double[] ah) throws DivideByZeroException { 116 double[] xc = new double[ah.length - 1]; 117 final double s = 1 / ah[ah.length - 1]; 118 if (!Double.isFinite(s)) // isZero(s) 119 throw new DivideByZeroException(); 120 for (int i = 0; i < ah.length - 1; i++) { 121 xc[i] = s * ah[i]; 122 } 123 return xc; 124 } 125 126 public static double mean(double[] x) { 127 final int n = x.length; 128 if (n == 0) 129 return 0; 130 double sum = 0; 131 for (int i = 0; i < x.length; i++) { 132 sum = sum + x[i]; 133 } 134 return sum / n; 135 } 136 137 /** 138 * Returns the variance of the specified values. 139 * @param x a sequence of real values 140 * @return the variance of the values in x (sigma^2) 141 */ 142 public static double variance(double[] x) { 143 final int n = x.length; 144 if (n == 0) 145 return 0; 146 double sum = 0; 147 double sum2 = 0; 148 for (int i = 0; i < x.length; i++) { 149 sum = sum + x[i]; 150 sum2 = sum2 + x[i] * x[i]; 151 } 152 return (sum2 - (sum * sum) / n) / n; 153 } 154 155 // --------------------------------------------------------------- 156 157 /** 158 * Converts a {@link Rotation} to a {@link Quaternion}. 159 * @param R a rotation 160 * @return the corresponding quaternion 161 */ 162 public static Quaternion toQuaternion(Rotation R) { 163 return new Quaternion(R.getQ0(), R.getQ1(), R.getQ2(), R.getQ3()); 164 } 165 166 /** 167 * Converts a {@link Quaternion} to a {@link Rotation}. 168 * @param q a quaternion 169 * @return the associated rotation 170 */ 171 public static Rotation toRotation(Quaternion q) { 172 return new Rotation(q.getQ0(), q.getQ1(), q.getQ2(), q.getQ3(), true); 173 } 174 175 /** 176 * Linearly interpolate two 3D {@link Rotation} instances. 177 * @param R0 first rotation 178 * @param R1 second rotation 179 * @param alpha the blending factor in [0,1] 180 * @return the interpolated rotation 181 */ 182 public static Rotation Lerp(Rotation R0, Rotation R1, double alpha) { 183 Quaternion qa = toQuaternion(R0); 184 Quaternion qb = toQuaternion(R1); 185 return toRotation(Lerp(qa, qb, alpha)); 186 } 187 188 /** 189 * Linearly interpolate two {@link Quaternion} instances. 190 * @param Q0 first quaternion 191 * @param Q1 second quaternion 192 * @param alpha the blending factor in [0,1] 193 * @return the interpolated quaternion 194 */ 195 public static Quaternion Lerp(Quaternion Q0, Quaternion Q1, double alpha) { 196 return Quaternion.add(Q0.multiply(1 - alpha), Q1.multiply(alpha)); 197 } 198 199 /** 200 * Linearly interpolate two 3D translation vectors. 201 * @param t0 first translation vector 202 * @param t1 second translation vector 203 * @param alpha the blending factor in [0,1] 204 * @return the interpolated translation vector 205 */ 206 public static double[] Lerp(double[] t0, double[] t1, double alpha) { 207 double[] t01 = t0.clone(); 208 for (int i = 0; i < t01.length; i++) { 209 t01[i] = (1 - alpha) * t0[i] + alpha * t1[i]; 210 } 211 return t01; 212 } 213 214 // --------------------------------------------------------------- 215 216 /** 217 * Calculates the 'inverse condition number' (RCOND() in Matlab) 218 * of the given matrix (0,...,1, ideally close to 1). 219 * @param A the matrix 220 * @return the inverse condition number 221 */ 222 public static double inverseConditionNumber(double[][] A) { 223 RealMatrix M = new Array2DRowRealMatrix(A); 224 SingularValueDecomposition svd = new SingularValueDecomposition(M); 225 return svd.getInverseConditionNumber(); 226 } 227 228 229 // --------------------------------------------------------------- 230 231 // /** 232 // * For testing only. 233 // * @param args ignored 234 // */ 235 // public static void main (String[] args) { 236 // //double[][] A = {{1, 2, 3}, {4, 5, 6}, {9, 8, 0}}; 237 // double[][] A = {{1, 2, 3}, {4, 5, 6}, {9, 8, 0}, {-3, 7, 2}}; 238 // { 239 // RealMatrix AM = MatrixUtils.createRealMatrix(A); 240 // RealVector x = solveHomogeneousSystem(AM); 241 // System.out.println("Solution x = " + x.toString()); 242 // } 243 // // Solution x = {0.649964237; -0.7338780288; 0.1974070146} 244 // } 245}