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); } }
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]);
/** * 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]); }
@Override public void unsafe_set(int row, int col, double value) { int index = nz_index(row,col); if( index < 0 ) addItem( row,col,value); else { nz_value.data[index] = value; } }
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; } }
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(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 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; }