/** * Inserts 3-tuple into the camera matrix's columns */ public static void insertColumn(DMatrixRMaj P, int col, GeoTuple3D_F64 a) { P.unsafe_set(0,col,a.x); P.unsafe_set(1,col,a.y); P.unsafe_set(2,col,a.z); }
/** * Constructs the linear system which is to be solved. * * sum a_ij*x_j - a_ij*u_i*z_j = 0 * sum a_ij*y_j - a_ij*v_i*z_j = 0 * * where (x,y,z) is the control point to be solved for. * (u,v) is the observed normalized point * */ protected static void constructM(List<Point2D_F64> obsPts, DMatrixRMaj alphas, DMatrixRMaj M) { int N = obsPts.size(); M.reshape(3*alphas.numCols,2*N,false); for( int i = 0; i < N; i++ ) { Point2D_F64 p2 = obsPts.get(i); int row = i*2; for( int j = 0; j < alphas.numCols; j++ ) { int col = j*3; double alpha = alphas.unsafe_get(i, j); M.unsafe_set(col, row, alpha); M.unsafe_set(col + 1, row, 0); M.unsafe_set(col + 2, row, -alpha * p2.x); M.unsafe_set(col , row + 1, 0); M.unsafe_set(col + 1, row + 1, alpha); M.unsafe_set(col + 2, row + 1, -alpha * p2.y); } } }
@Override public DMatrixRMaj getT(DMatrixRMaj T) { int N = Ablock.numRows; if( T == null ) { T = new DMatrixRMaj(N,N); } else { CommonOps_DDRM.fill(T, 0); } double[] diag = new double[ N ]; double[] off = new double[ N ]; ((TridiagonalDecompositionHouseholder_DDRB)alg).getDiagonal(diag,off); T.unsafe_set(0,0,diag[0]); for( int i = 1; i < N; i++ ) { T.unsafe_set(i,i,diag[i]); T.unsafe_set(i,i-1,off[i-1]); T.unsafe_set(i-1,i,off[i-1]); } return T; }
A.unsafe_set(i,0,s_norm.x*f_norm.x); A.unsafe_set(i,1,s_norm.x*f_norm.y); A.unsafe_set(i,2,s_norm.x); A.unsafe_set(i,3,s_norm.y*f_norm.x); A.unsafe_set(i,4,s_norm.y*f_norm.y); A.unsafe_set(i,5,s_norm.y); A.unsafe_set(i,6,f_norm.x); A.unsafe_set(i,7,f_norm.y); A.unsafe_set(i,8,1);
/** * Writes the lower triangular matrix into the specified matrix. * * @param lower Where the lower triangular matrix is written to. */ @Override public DMatrixRMaj getLower(DMatrixRMaj lower ) { int numRows = LU.numRows; int numCols = LU.numRows < LU.numCols ? LU.numRows : LU.numCols; lower = UtilDecompositons_DDRM.checkZerosUT(lower,numRows,numCols); for( int i = 0; i < numCols; i++ ) { lower.unsafe_set(i,i,1.0); for( int j = 0; j < i; j++ ) { lower.unsafe_set(i,j, LU.unsafe_get(i,j)); } } if( numRows > numCols ) { for( int i = numCols; i < numRows; i++ ) { for( int j = 0; j < numCols; j++ ) { lower.unsafe_set(i,j, LU.unsafe_get(i,j)); } } } return lower; }
/** * Retrieve projective transform H */ public void computeH( DMatrixRMaj H ) { H.reshape(4,4); CommonOps_DDRM.insert(PA,H,0,0); for (int i = 0; i < 4; i++) { H.unsafe_set(i,3,ns.data[i]); } }
/** * @see #diag(double...) */ public static DMatrixRMaj diag(DMatrixRMaj ret , int width , double ...diagEl ) { if( ret == null ) { ret = new DMatrixRMaj(width,width); } else { if( ret.numRows != width || ret.numCols != width ) throw new IllegalArgumentException("Unexpected matrix size"); CommonOps_DDRM.fill(ret, 0); } for( int i = 0; i < width; i++ ) { ret.unsafe_set(i, i, diagEl[i]); } return ret; }
private void copyZeros( DSubmatrixD1 subU ) { int N = Math.min(A.blockLength,subU.col1-subU.col0); for( int i = 0; i < N; i++ ) { // save the zeros for( int j = 0; j <= i; j++ ) { zerosM.unsafe_set(i,j,subU.get(i,j)); subU.set(i,j,0); } // save the one if( subU.col0 + i + 1 < subU.original.numCols ) { zerosM.unsafe_set(i,i+1,subU.get(i,i+1)); subU.set(i,i+1,1); } } }
@Override public void process(double[] input, DMatrixRMaj jacobian) { DMatrixRMaj J = jacobian; for( int i = 0; i < N; i++ ) { double x = input[i]; double h = x != 0 ? differenceScale*Math.abs(x) : differenceScale; // backwards sample double temp0 = x-h; input[i] = temp0; double h0 = x-temp0; function.process(input,output0); // forwards sample double temp1 = x+h; double h1 = temp1-x; input[i] = temp1; function.process(input,output1); for( int j = 0; j < M; j++ ) { J.unsafe_set(j,i,(output1[j] - output0[j])/(h0+h1)); } input[i] = x; } }
@Override public void process(double[] input, DMatrixRMaj jacobian) { DMatrixRMaj J = jacobian; for( int i = 0; i < N; i++ ) { double x = input[i]; double h = x != 0 ? differenceScale*Math.abs(x) : differenceScale; // backwards sample double temp0 = x-h; input[i] = temp0; double h0 = x-temp0; function.process(input,output0); // forwards sample double temp1 = x+h; double h1 = temp1-x; input[i] = temp1; function.process(input,output1); for( int j = 0; j < M; j++ ) { J.unsafe_set(j,i,(output1[j] - output0[j])/(h0+h1)); } input[i] = x; } }
/** * Writes the upper triangular matrix into the specified matrix. * * @param upper Where the upper triangular matrix is writen to. */ @Override public DMatrixRMaj getUpper(DMatrixRMaj upper ) { int numRows = LU.numRows < LU.numCols ? LU.numRows : LU.numCols; int numCols = LU.numCols; upper = UtilDecompositons_DDRM.checkZerosLT(upper,numRows,numCols); for( int i = 0; i < numRows; i++ ) { for( int j = i; j < numCols; j++ ) { upper.unsafe_set(i,j, LU.unsafe_get(i,j)); } } return upper; }
@Override public DMatrixRMaj getT(DMatrixRMaj T ) { // write the values to T if( lower ) { T = UtilDecompositons_DDRM.checkZerosUT(T,n,n); for( int i = 0; i < n; i++ ) { for( int j = 0; j <= i; j++ ) { T.unsafe_set(i,j,this.T.unsafe_get(i,j)); } } } else { T = UtilDecompositons_DDRM.checkZerosLT(T,n,n); for( int i = 0; i < n; i++ ) { for( int j = i; j < n; j++ ) { T.unsafe_set(i,j,this.T.unsafe_get(i,j)); } } } return T; }
@Override public void process(double[] input, DMatrixRMaj jacobian) { jacobian.reshape(M,N); function.process(input,output0); for( int i = 0; i < N; i++ ) { double x = input[i]; double h = x != 0 ? differenceScale*Math.abs(x) : differenceScale; // takes in account round off error double temp = x+h; h = temp-x; input[i] = temp; function.process(input,output1); for( int j = 0; j < M; j++ ) { jacobian.unsafe_set(j,i,(output1[j] - output0[j])/h); } input[i] = x; } }
@Override public void process(double[] input, DMatrixRMaj jacobian) { jacobian.reshape(M,N); function.process(input,output0); for( int i = 0; i < N; i++ ) { double x = input[i]; double h = x != 0 ? differenceScale*Math.abs(x) : differenceScale; // takes in account round off error double temp = x+h; h = temp-x; input[i] = temp; function.process(input,output1); for( int j = 0; j < M; j++ ) { jacobian.unsafe_set(j,i,(output1[j] - output0[j])/h); } input[i] = x; } }
@Override public DMatrixRMaj getW(DMatrixRMaj W ) { int m = compact ? numSingular : numRows; int n = compact ? numSingular : numCols; if( W == null ) W = new DMatrixRMaj(m,n); else { W.reshape(m,n, false); W.zero(); } for( int i = 0; i < numSingular; i++ ) { W.unsafe_set(i,i, singularValues[i]); } return W; }
public static DMatrixRMaj convert(DMatrixSparseTriplet src , DMatrixRMaj dst ) { if( dst == null ) dst = new DMatrixRMaj(src.numRows, src.numCols); else { dst.reshape(src.numRows, src.numCols); dst.zero(); } for (int i = 0; i < src.nz_length; i++) { int row = src.nz_rowcol.data[i*2]; int col = src.nz_rowcol.data[i*2+1]; double value = src.nz_value.data[i]; dst.unsafe_set(row, col, value); } return dst; }
/** * Projects the found estimate of E onto essential space. * * @return true if svd returned true. */ protected boolean projectOntoEssential( DMatrixRMaj E ) { if( !svdConstraints.decompose(E) ) { return false; } svdV = svdConstraints.getV(svdV,false); svdU = svdConstraints.getU(svdU,false); svdS = svdConstraints.getW(svdS); SingularOps_DDRM.descendingOrder(svdU, false, svdS, svdV, false); // project it into essential space // the scale factor is arbitrary, but the first two singular values need // to be the same. so just set them to one svdS.unsafe_set(0, 0, 1); svdS.unsafe_set(1, 1, 1); svdS.unsafe_set(2, 2, 0); // recompute F CommonOps_DDRM.mult(svdU, svdS, temp0); CommonOps_DDRM.multTransB(temp0,svdV, E); return true; }
int min = Math.min(i,R.numCols); for( int j = 0; j < min; j++ ) { R.unsafe_set(i,j,0); R.unsafe_set(i,j,QR.unsafe_get(j,i));
public static DMatrixRMaj convert(DMatrixSparseCSC src , DMatrixRMaj dst ) { if( dst == null ) dst = new DMatrixRMaj(src.numRows, src.numCols); else { dst.reshape(src.numRows, src.numCols); dst.zero(); } int idx0 = src.col_idx[0]; for (int j = 1; j <= src.numCols; j++) { int idx1 = src.col_idx[j]; for (int i = idx0; i < idx1; i++) { int row = src.nz_rows[i]; double val = src.nz_values[i]; dst.unsafe_set(row,j-1, val); } idx0 = idx1; } return dst; }
/** * Compute projective transform that converts P into identity * @param P (Input) 3x4 camera matrix * @return true if no errors */ public boolean process( DMatrixRMaj P ) { if( !svd.decompose(P) ) return false; svd.getU(Ut,true); svd.getV(V,false); double sv[] = svd.getSingularValues(); SingularOps_DDRM.descendingOrder(Ut,true,sv,3,V,false); // compute W+, which is transposed and non-negative inverted for (int i = 0; i < 3; i++) { Wt.unsafe_set(i,i, 1.0/sv[i]); } // get the pseudo inverse // A+ = V*(W+)*U' CommonOps_DDRM.mult(V,Wt,tmp); CommonOps_DDRM.mult(tmp, Ut,PA); // Vector U, which is P*U = 0 SpecializedOps_DDRM.subvector(V,0,3,V.numRows,false,0,ns); return true; }