fuhito615.hatenablog.com
上記記事でホモグラフィ変換を実装した際、行列の計算にはND4Jライブラリを使用しました。
が、どうせなら行列クラスも自前で実装してしみようと思い立ち、実装しました。
ソースコード
行列クラス
public class Matrix { protected double[][] mat; protected int rowSize; protected int columnSize; /** * コンストラクタ * @param rowSize 行サイズ * @param columnSize 列サイズ */ public Matrix(int rowSize, int columnSize) { this.rowSize = rowSize; this.columnSize = columnSize; this.mat = new double[rowSize][columnSize]; } /** * コンストラクタ * @param arr 行列を表すdouble配列 */ public Matrix(double[][] arr) { this.rowSize = arr.length; this.columnSize = arr[0].length; for (int i = 0; i < this.rowSize; i++) if (arr[i].length != this.columnSize) throw new IllegalArgumentException(); this.mat = arr; } /** * 行列を表すdouble配列を取得する。 * @return */ public double[][] getDouble() { double[][] rtn = new double[rowSize][columnSize]; for (int i = 0; i < rowSize; i++) for (int j = 0; j < columnSize; j++) rtn[i][j] = mat[i][j]; return rtn; } /** * 同じサイズか。 * @param matrix 比較する行列 * @return 同じサイズか */ public boolean isSameSize(Matrix matrix) { return rowSize == matrix.rowSize && columnSize == matrix.columnSize; } @Override public Matrix clone() { double[][] cloneMat = new double[rowSize][columnSize]; for (int i = 0; i < rowSize; i++) for (int j = 0; j < columnSize; j++) cloneMat[i][j] = mat[i][j]; return new Matrix(cloneMat); } /** * ピボット操作。 * @param rowIndex 対象行 */ protected void pivoting(int rowIndex) { int columnIndex = rowIndex; // 対象行より下の行の中で、絶対値が最大の行と入れ替える double max = Math.abs(mat[rowIndex][columnIndex]); int maxRowIndex = rowIndex; for (int i = rowIndex+1; i < rowSize; i++) { double tmp = Math.abs(mat[i][columnIndex]); if (tmp > max) { max = tmp; maxRowIndex = i; } } if (rowIndex != maxRowIndex) swapRow(rowIndex, maxRowIndex); } /** * 行列の基本変形(1) * 1つの行をスカラー倍（≠0）する。 * @param rowIndex スカラー倍する行 * @param scalar スカラー */ public void multiplyRow(int rowIndex, double scalar) { if (scalar == 0) throw new IllegalArgumentException(); for (int j = 0; j < columnSize; j++) mat[rowIndex][j] *= scalar; } /** * 行列の基本変形(2) * 2つの行を入れ替える。 * @param rowIndex1 入れ替える行 * @param rowIndex2 入れ替える行 */ public void swapRow(int rowIndex1, int rowIndex2) { double[] swap = mat[rowIndex1]; mat[rowIndex1] = mat[rowIndex2]; mat[rowIndex2] = swap; } /** * 行列の基本変形(3) * 1つの行に他の行のスカラー倍を加える。 * @param augendRowIndex 足される行 * @param addendRowIndex 足す行 * @param scalar スカラー（足す行に掛ける数） */ public void addMultipliedOtherRow(int augendRowIndex, int addendRowIndex, double scalar) { for (int j = 0; j < columnSize; j++) mat[augendRowIndex][j] += mat[addendRowIndex][j] * scalar; } /** * 転置行列を取得する。 * @return 転置行列 */ public Matrix transpose() { double[][] tMat = new double[columnSize][rowSize]; for (int i = 0; i < rowSize; i++) for (int j = 0; j < columnSize; j++) tMat[j][i] = mat[i][j]; return new Matrix(tMat); } /** * 行列の足し算。 * @param matrix1 足される行列 * @param matrix2 足す行列 * @return 計算結果 */ public static Matrix add(Matrix matrix1, Matrix matrix2) { if (!matrix1.isSameSize(matrix2)) throw new IllegalArgumentException(); double[][] result = new double[matrix1.rowSize][matrix1.columnSize]; double[][] mat1 = matrix1.mat; double[][] mat2 = matrix2.mat; for (int i = 0; i < matrix1.rowSize; i++) for (int j = 0; j < matrix1.columnSize; j++) result[i][j] = mat1[i][j] + mat2[i][j]; return new Matrix(result); } /** * 行列の引き算。 * @param matrix1 引かれる行列 * @param matrix2 引く行列 * @return 計算結果 */ public static Matrix sub(Matrix matrix1, Matrix matrix2) { if (!matrix1.isSameSize(matrix2)) throw new IllegalArgumentException(); double[][] result = new double[matrix1.rowSize][matrix1.columnSize]; double[][] mat1 = matrix1.mat; double[][] mat2 = matrix2.mat; for (int i = 0; i < matrix1.rowSize; i++) for (int j = 0; j < matrix1.columnSize; j++) result[i][j] = mat1[i][j] - mat2[i][j]; return new Matrix(result); } /** * 行列の掛け算。 * @param matrix1 掛けられる行列 * @param matrix2 掛ける行列 * @return 計算結果 */ public static Matrix matmul(Matrix matrix1, Matrix matrix2) { Matrix tMatrix2 = matrix2.transpose(); if (matrix1.columnSize != tMatrix2.columnSize) throw new IllegalArgumentException(); double[][] result = new double[matrix1.rowSize][tMatrix2.rowSize]; double[][] mat1 = matrix1.mat; double[][] mat2 = tMatrix2.mat; for (int i = 0; i < matrix1.rowSize; i++) { for (int j = 0; j < tMatrix2.rowSize; j++) { result[i][j] = 0; for (int k = 0; k < matrix1.columnSize; k++) result[i][j] += mat1[i][k] * mat2[j][k]; } } return new Matrix(result); } /** * 行列を横方向に連結する。 * @param matrix1 左側の行列 * @param matrix2 右側の行列 * @return 連結後の行列 */ public static Matrix concatH(Matrix matrix1, Matrix matrix2) { if (matrix1.rowSize != matrix2.rowSize) throw new IllegalArgumentException(); int concatRowSize = matrix1.rowSize; int concatColumnSize = matrix1.columnSize + matrix2.columnSize; double[][] result = new double[concatRowSize][concatColumnSize]; double[][] mat1 = matrix1.mat; double[][] mat2 = matrix2.mat; for (int i = 0; i < concatRowSize; i++) { for (int j = 0; j < concatColumnSize; j++) { result[i][j] = (j < matrix1.columnSize) ? mat1[i][j] : mat2[i][j - matrix1.columnSize]; } } return new Matrix(result); } @Override public String toString() { StringBuilder sb = new StringBuilder(""); for (int i = 0; i < rowSize; i++) { sb.append("[ "); for (int j = 0; j < columnSize; j++) { sb.append(String.format("%+.5f", mat[i][j]).replace("+", " ")); sb.append(", "); } sb.append("]"); sb.append(System.lineSeparator()); } return sb.toString(); } }
正方行列クラス
public class SquareMatrix extends Matrix { private int size; /** * コンストラクタ * @param size サイズ */ public SquareMatrix(int size) { super(size, size); this.size = size; } /** * コンストラクタ * @param arr 正方行列を表すdouble配列 */ public SquareMatrix(double[][] arr) { super(arr); if (rowSize != columnSize) throw new IllegalArgumentException(); this.size = rowSize; } /** * 単位行列を取得する。 * @param size サイズ * @return 単位行列 */ public static SquareMatrix identityMatrix(int size) { SquareMatrix iMatrix = new SquareMatrix(size); for (int i = 0; i < size; i++) for (int j = 0; j < size; j++) iMatrix.mat[i][j] = (i==j) ? 1.0 : 0.0; return iMatrix; } /** * 逆行列を取得する。 * （掃き出し法） * @return 逆行列 */ public SquareMatrix inverse() { Matrix wk = Matrix.concatH(clone(), SquareMatrix.identityMatrix(size)); for (int i = 0; i < rowSize; i++) { // 1. ピボット操作 wk.pivoting(i); // 2. 対角成分を1にするために割る double div = wk.mat[i][i]; wk.multiplyRow(i, 1/div); // 3. 他行の同列成分を0にする for (int i2 = 0; i2 < rowSize; i2++) { if (i2 == i) continue; double mul = wk.mat[i2][i] * (-1); if (mul == 0) continue; wk.addMultipliedOtherRow(i2, i, mul); } } double[][] invArr = new double[size][size]; for (int i = 0; i < size; i++) for (int j = 0; j < size; j++) invArr[i][j] = wk.mat[i][size+j]; return new SquareMatrix(invArr); } @Override public SquareMatrix clone() { double[][] cloneMat = new double[size][size]; for (int i = 0; i < size; i++) for (int j = 0; j < size; j++) cloneMat[i][j] = mat[i][j]; return new SquareMatrix(cloneMat); } }
行列の積の計算、逆行列の計算さえあれば一旦は事足りるため、最低限で実装。
逆行列の計算は掃き出し法で実装しました。
行列の右側に単位行列をくっつけて、左側が単位行列となるように行の基本変形を繰り返し行う、という求め方です。
逆行列の計算方法は掃き出し法の他に「LU分解」を用いる方法があるようです。
ぱっと見て理解できなかったので今回は諦めて掃き出し法にしました。
LU分解を用いて求める方が掃き出し法より計算量が少ないみたいなので、覚えていたら暇なときに改善してみます。
プログラムの実行
適当な行列を逆行列に変換し、元の行列と逆行列の積をとって単位行列となることを確認します。
SquareMatrix m = new SquareMatrix(new double[][] { {1,2,3}, {8,9,4}, {7,6,5}, }); System.out.println(m); SquareMatrix inv = m.inverse(); System.out.println(inv); System.out.println(Matrix.matmul(m, inv));
実行結果。
[ 1.00000, 2.00000, 3.00000, ] [ 8.00000, 9.00000, 4.00000, ] [ 7.00000, 6.00000, 5.00000, ] [ -0.43750, -0.16667, 0.39583, ] [ 0.25000, 0.33333, -0.41667, ] [ 0.31250, -0.16667, 0.14583, ] [ 1.00000, 0.00000, 0.00000, ] [ 0.00000, 1.00000, 0.00000, ] [ 0.00000, -0.00000, 1.00000, ]
問題なく実装できていたため、ホモグラフィ変換のコードに反映しました。
/** * ホモグラフィ変換行列を設定する。 * @param src 変換前の4座標 * @param dst 変換後の4座標 */ public void setParam(double[][] src, double[][] dst) { if (src.length != 4) throw new IllegalArgumentException(); if (dst.length != 4) throw new IllegalArgumentException(); for (int i = 0; i < src.length; i++) if (src[i].length != 2) throw new IllegalArgumentException(); for (int i = 0; i < dst.length; i++) if (dst[i].length != 2) throw new IllegalArgumentException(); SquareMatrix mat = new SquareMatrix(new double[][] { { src[0][0], src[0][1], 1, 0, 0, 0, -1*src[0][0]*dst[0][0], -1*src[0][1]*dst[0][0] }, { 0, 0, 0, src[0][0], src[0][1], 1, -1*src[0][0]*dst[0][1], -1*src[0][1]*dst[0][1] }, { src[1][0], src[1][1], 1, 0, 0, 0, -1*src[1][0]*dst[1][0], -1*src[1][1]*dst[1][0] }, { 0, 0, 0, src[1][0], src[1][1], 1, -1*src[1][0]*dst[1][1], -1*src[1][1]*dst[1][1] }, { src[2][0], src[2][1], 1, 0, 0, 0, -1*src[2][0]*dst[2][0], -1*src[2][1]*dst[2][0] }, { 0, 0, 0, src[2][0], src[2][1], 1, -1*src[2][0]*dst[2][1], -1*src[2][1]*dst[2][1] }, { src[3][0], src[3][1], 1, 0, 0, 0, -1*src[3][0]*dst[3][0], -1*src[3][1]*dst[3][0] }, { 0, 0, 0, src[3][0], src[3][1], 1, -1*src[3][0]*dst[3][1], -1*src[3][1]*dst[3][1] } }); Matrix dstArray = new Matrix(new double[][] { {dst[0][0], dst[0][1], dst[1][0], dst[1][1], dst[2][0], dst[2][1], dst[3][0], dst[3][1]}, }); // ( a, b, c, d, e, f, g, h ) Matrix result = Matrix.matmul(mat.inverse(), dstArray.transpose()); this.param = result.transpose().getDouble()[0]; }
