public static DMatrixSparseTriplet convert(DMatrixSparseCSC src , DMatrixSparseTriplet dst ) { if( dst == null ) dst = new DMatrixSparseTriplet(src.numRows, src.numCols, src.nz_length); else dst.reshape( src.numRows , src.numCols ); int i0 = src.col_idx[0]; for (int col = 0; col < src.numCols; col++) { int i1 = src.col_idx[col+1]; for (int i = i0; i < i1; i++) { int row = src.nz_rows[i]; dst.addItem(row,col, src.nz_values[i]); } i0 = i1; } return dst; } }
public DMatrixSparseTriplet(DMatrixSparseTriplet orig ) { set(orig); }
@Override public void process(double[] input, DMatrixSparseCSC jacobian) { jacobian.reshape(M,N,N); function.process(input,output0); // Use a triplet initially because it is less expensive to grow DMatrixSparseTriplet tmp = new DMatrixSparseTriplet(M,N,N); 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++ ) { double value = (output1[j] - output0[j])/h; if( Math.abs(value) > zeroTolerance ) tmp.set(j,i,value); } input[i] = x; } ConvertDMatrixStruct.convert(tmp,jacobian); }
private DMatrixSparseTriplet readDSTR(int numRows, int numCols, int length) throws IOException { List<String> words; DMatrixSparseTriplet m = new DMatrixSparseTriplet(numRows,numCols,length); for (int i = 0; i < length; i++) { words = extractWords(); if( words.size() != 3 ) throw new IllegalArgumentException("Unexpected number of words on line "+getLineNumber()); int row = Integer.parseInt(words.get(0)); int col = Integer.parseInt(words.get(1)); double value = Double.parseDouble(words.get(2)); m.addItem(row,col,value); } return m; } }
int numViewParam = numParameters-numPointParam; // view + camera tripletPoint.reshape(numRows,numPointParam); tripletView.reshape(numRows,numViewParam); tripletView.addItemCheck(jacRowX,location+j,calibGradX[j]); tripletView.addItemCheck(jacRowY,location+j,calibGradY[j]); tripletView.addItemCheck(jacRowX,col+3, pointGradX[0]); tripletView.addItem(jacRowY,col+3, pointGradY[0]); tripletView.addItemCheck(jacRowX,col+4, pointGradX[1]); tripletView.addItem(jacRowY,col+4, pointGradY[1]); tripletView.addItemCheck(jacRowX,col+5, pointGradX[2]); tripletView.addItem(jacRowY,col+5, pointGradY[2]);
DMatrixSparseTriplet tripletLeft = new DMatrixSparseTriplet(N,2*numCameras,1); DMatrixSparseTriplet tripletRight = new DMatrixSparseTriplet(N,2*numLandmarks,1); double dy = -1.0/bottom; tripletLeft.addItemCheck(output,i*2+0,a*dx); tripletLeft.addItemCheck(output,i*2+1,a*dy); tripletRight.addItemCheck(output,i*2+0,a*dx); tripletRight.addItemCheck(output,i*2+1,a*dy);
@Override public <T extends Matrix> T createLike() { return (T)new DMatrixSparseTriplet(numRows,numCols, nz_length); }
@Override public void set(Matrix original) { DMatrixSparseTriplet orig = (DMatrixSparseTriplet)original; reshape(orig.numRows,orig.numCols); this.nz_rowcol.set(orig.nz_rowcol); this.nz_value.set(orig.nz_value); this.nz_length = orig.nz_length; }
private void addToJacobian(DMatrixSparseTriplet tripplet, int col , double a[], double b[], DMatrixRMaj R , Point3D_F64 X ) { double x = R.data[0]*X.x + R.data[1]*X.y + R.data[2]*X.z; double y = R.data[3]*X.x + R.data[4]*X.y + R.data[5]*X.z; double z = R.data[6]*X.x + R.data[7]*X.y + R.data[8]*X.z; tripplet.addItem(jacRowX,col,a[0]*x + a[1]*y + a[2]*z); tripplet.addItem(jacRowY,col,b[0]*x + b[1]*y + b[2]*z); } }
/** * Saves a matrix to disk using in a Column Space Value (CSV) format. For a * description of the format see {@link MatrixIO#loadCSV(String,boolean)}. * * @param A The matrix being saved. * @param fileName Name of the file its being saved at. * @throws java.io.IOException */ public static void saveSparseCSV(DMatrixSparseTriplet A , String fileName ) throws IOException { PrintStream fileStream = new PrintStream(fileName); fileStream.println(A.getNumRows() + " " + A.getNumCols() +" "+A.nz_length+ " real"); for (int i = 0; i < A.nz_length; i++) { int row = A.nz_rowcol.data[i*2]; int col = A.nz_rowcol.data[i*2+1]; double value = A.nz_value.data[i]; fileStream.println(row+" "+col+" "+value); } fileStream.close(); }
@Override public void process(double[] input, DMatrixSparseCSC jacobian) { jacobian.reshape(M,N,N); function.process(input,output0); // Use a triplet initially because it is less expensive to grow DMatrixSparseTriplet tmp = new DMatrixSparseTriplet(M,N,N); 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++ ) { double value = (output1[j] - output0[j])/h; if( Math.abs(value) > zeroTolerance ) tmp.set(j,i,value); } input[i] = x; } ConvertDMatrixStruct.convert(tmp,jacobian); }
@Override public <T extends Matrix> T copy() { return (T)new DMatrixSparseTriplet(this); }
@Override public void reshape(int numRows, int numCols, int arrayLength) { reshape(numRows, numCols); nz_rowcol.reshape(arrayLength*2); nz_value.reshape(arrayLength); }
/** * J[rows,col:(col+3)] = [a;b]*R */ private void addToJacobian(DMatrixSparseTriplet tripplet, int col , double a[], double b[], DMatrixRMaj R ) { tripplet.addItem(jacRowX,col+0,a[0]*R.data[0] + a[1]*R.data[3] + a[2]*R.data[6]); tripplet.addItem(jacRowX,col+1,a[0]*R.data[1] + a[1]*R.data[4] + a[2]*R.data[7]); tripplet.addItem(jacRowX,col+2,a[0]*R.data[2] + a[1]*R.data[5] + a[2]*R.data[8]); tripplet.addItem(jacRowY,col+0,b[0]*R.data[0] + b[1]*R.data[3] + b[2]*R.data[6]); tripplet.addItem(jacRowY,col+1,b[0]*R.data[1] + b[1]*R.data[4] + b[2]*R.data[7]); tripplet.addItem(jacRowY,col+2,b[0]*R.data[2] + b[1]*R.data[5] + b[2]*R.data[8]); }
public static DMatrixSparseTriplet convert(DMatrixRMaj src , DMatrixSparseTriplet dst , double tol ) { if( dst == null ) dst = new DMatrixSparseTriplet(src.numRows, src.numCols,src.numRows*src.numCols); else dst.reshape(src.numRows, src.numCols); int index = 0; for (int row = 0; row < src.numRows; row++) { for (int col = 0; col < src.numCols; col++) { double value = src.data[index++]; if( Math.abs(value) > tol ) dst.addItem(row,col,value); } } return dst; }
public static DMatrixSparseTriplet convert(DMatrix src , DMatrixSparseTriplet dst , double tol ) { if( dst == null ) dst = new DMatrixSparseTriplet(src.getNumRows(), src.getNumCols(), 1); else dst.reshape(src.getNumRows(), src.getNumCols()); for (int row = 0; row < src.getNumRows(); row++) { for (int col = 0; col < src.getNumCols(); col++) { double value = src.unsafe_get(row,col); if( Math.abs(value) > tol ) dst.addItem(row,col,value); } } return dst; }