@Override public <T extends Matrix> T copy() { return (T)new DMatrix3(this); }
/** * Computes rotators which rotate t into [|t|,0,0] * * @param t Input the vector, Output vector after rotator has been applied */ static void computeRotation(DMatrix3 t , DMatrix3x3 R ) { for (int i = 1; i >= 0; i--) { double a = t.get(i,0); double b = t.get(i+1,0); // compute the rotator such that [a,b] = [||X||,0] double r = Math.sqrt(a*a + b*b); double q11 = a/r; double q21 = b/r; // apply rotator to t and R t.set(i,0,r); t.set(i+1,0,0); if( i == 1 ) { R.a11 = 1; R.a12 = 0; R.a13 = 0; R.a21 = 0; R.a22 = q11; R.a23 = q21; R.a31 = 0; R.a32 = -q21; R.a33 = q11; } else { R.a11 = q11; R.a12 = R.a22*q21; R.a13 = R.a23*q21; R.a21 = -q21; R.a22 = R.a22*q11; R.a23 = R.a23*q11; } } }
@Override public double get(int row, int col) { return unsafe_get(row,col); }
/** * Decomposes the absolute dual quadratic into the following submatrices: Q=[w -w*p;-p'*w p'*w*p] * * @see DecomposeAbsoluteDualQuadratic * * @param Q (Input) Absolute quadratic. Typically found in auto calibration. Not modified. * @param w (Output) 3x3 symmetric matrix * @param p (Output) 3x1 vector * @return true if successful or false if it failed */ public static boolean decomposeAbsDualQuadratic( DMatrix4x4 Q , DMatrix3x3 w , DMatrix3 p ) { DecomposeAbsoluteDualQuadratic alg = new DecomposeAbsoluteDualQuadratic(); if( !alg.decompose(Q) ) return false; w.set(alg.getW()); p.set(alg.getP()); return true; }
@Override public void set(int row, int col, double val) { unsafe_set(row,col,val); }
@Override public void process(double[] input, double[] output) { p.set( input[0], input[1] , input[2] ); int indexInput = encodeK(K,0,3,input); eq.alias(p,"p",K,"K"); eq.process("w0=K*K'"); int indexOut = 0; for (int i = 1; i < cameras.size; i++) { Projective P = cameras.get(i); indexInput = encodeK(K,i,indexInput,param.data); eq.alias(K,"K",P.A,"A",P.a,"a"); eq.process("AP = A-a*p'"); eq.process("kk = K*K'/normF(K*K')"); eq.process("AW = AP*w0*AP'"); eq.process("AW = AW / normF(AW)"); eq.process("R = kk-AW"); DMatrixRMaj residual = eq.lookupDDRM("R"); output[indexOut++] = residual.get(0,0); output[indexOut++] = residual.get(0,1); output[indexOut++] = residual.get(0,2); output[indexOut++] = residual.get(1,1); output[indexOut++] = residual.get(1,2); output[indexOut++] = residual.get(2,2); } }
@Override public <T extends Matrix> T createLike() { return (T)new DMatrix3(); }
/** * Extracts the row from the matrix a. * @param a Input matrix * @param row Which row is to be extracted * @param out output. Storage for the extracted row. If null then a new vector will be returned. * @return The extracted row. */ public static DMatrix3 extractRow( DMatrix3x3 a , int row , DMatrix3 out ) { if( out == null) out = new DMatrix3(); switch( row ) { case 0: out.a1 = a.a11; out.a2 = a.a12; out.a3 = a.a13; break; case 1: out.a1 = a.a21; out.a2 = a.a22; out.a3 = a.a23; break; case 2: out.a1 = a.a31; out.a2 = a.a32; out.a3 = a.a33; break; default: throw new IllegalArgumentException("Out of bounds row. row = "+row); } return out; }
/** * Extracts the column from the matrix a. * @param a Input matrix * @param column Which column is to be extracted * @param out output. Storage for the extracted column. If null then a new vector will be returned. * @return The extracted column. */ public static DMatrix3 extractColumn( DMatrix3x3 a , int column , DMatrix3 out ) { if( out == null) out = new DMatrix3(); switch( column ) { case 0: out.a1 = a.a11; out.a2 = a.a21; out.a3 = a.a31; break; case 1: out.a1 = a.a12; out.a2 = a.a22; out.a3 = a.a32; break; case 2: out.a1 = a.a13; out.a2 = a.a23; out.a3 = a.a33; break; default: throw new IllegalArgumentException("Out of bounds column. column = "+column); } return out; }
/** * Converts {@link DMatrixRMaj} into {@link DMatrix3} * * @param input Input matrix. * @param output Output matrix. If null a new matrix will be declared. * @return Converted matrix. */ public static DMatrix3 convert(DMatrixRMaj input , DMatrix3 output ) { if( output == null) output = new DMatrix3(); if( input.getNumRows() != 1 && input.getNumCols() != 1 ) throw new IllegalArgumentException("One row or column must have a length of 1 for it to be a vector"); int length = Math.max(input.getNumRows(),input.getNumCols()); if( length != 3 ) throw new IllegalArgumentException("Length of input vector is not 3. It is "+length); output.a1 = input.data[0]; output.a2 = input.data[1]; output.a3 = input.data[2]; return output; }