JavaでOpenCVのカメラキャリブレーション(CameraCalibration)をやった

##はじめに
カメラキャリブレーションというものが必要になったのでやってみました。
注意: 筆者は独学で行なっているため、不正確な記述が含まれることがあります。

##カメラキャリブレーション(CameraCalibration)とは
空間の位置と画像上の位置を変換できる式がありますが、これはカメラのパラメーターが必要です。
カメラパラメーターには種類があり、大きく内部パラメーターと外部パラメーターに分けられます。
また、内部パラメーターに含まれるのかわからないですが、レンズ歪み係数(歪曲収差)というものもあります。

###内部パラメーター

内部パラメーターとは、「カメラによって決定されるパラメーター」です。
・焦点距離 f
・画素の物理的な間隔 δ<sub>u</sub>, δ<sub>v</sub>
・正規化座標に対する画像座標における画像中心 c<sub>u</sub>, c<sub>v</sub>

###外部パラメーター

外部パラメーターとは、「ワールド座標系に対するカメラの位置と姿勢のパラメーター」です。
・回転行列 R<sub>3</sub>
・平行移動ベクトル t<sub>3</sub>

###カメラキャリブレーションとは

さて、カメラキャリブレーションとは、このカメラのパラメーターを求めることです。
この際に得られる、内部パラメーターと外部パラメーターを合わせて、カメラ行列または透視投影行列と言います。
予め位置がわかっている空間点と、その画像上への投影点(つまり、画像上の座標)を行列の式に当てはめてパラメーターを決定します。

具体的に言えば、キャリブレーションターゲット(チェスボードなど)を動かしながら写真を撮影します。
チェスボードの格子の長さが既知であるため、格子点間の距離がわかり、空間上の位置が定まります。(逆に言えば、ワールド座標系を定めます)
また、画像上でも格子点の座標を取得します。(画像座標の取得)
画像を複数枚撮影し、最小二乗法を用いて、パラメーターを求めます。

引用(参考)元: 「ディジタル画像処理[改訂新版]」, なお都合より表現を改変している。
参考: [カメラキャリブレーションと3次元再構成 - OpenCV.jp](http://opencv.jp/opencv-2.1/cpp/camera_calibration_and_3d_reconstruction.html)
##OpenCVでカメラキャリブレーションを行う
今回は比較的簡単だと思う、チェストボードによるカメラキャリブレーションを以下の大まかな手順で行いました。

コードについては[OpenCV_CameraCalibration - GitHub](https://github.com/smk7758/OpenCV_CameraCalibration/blob/master/test-2018-12-10_OpenCV_CameraCalibration_2/src/com/github/smk7758/OpenCV_CameraCalibration/Main.java)を参照してください。

0. チェストボード(キャリブレーションターゲット)を持って、動かしながら写真を撮影します。
1. ワールド座標系と空間上の格子点の座標を定める
2. 画像上のチェストボードの格子点の座標を求める [Calib3d.findChessboardCorners()を利用]
3. Calib3d.calibrateCamera()を実行する
4. 返された カメラ行列とレンズ歪み係数を保存する
概要はこのような感じである。

それぞれについて少し詳しく解説します。

###ワールド座標系と空間上の格子点の座標を定める
ワールド座標系を、平面なキャリブレーションターゲット(チェストボード)の平面がZ=0となるように任意に定めます。
このときに格子点の間隔を測って、mm単位でPointを定めます。
ここで呼び出ししています。

```java
List<Mat> objectPoints = getObjectPoints(outputFindChessboardCorners.size(), patternSize); // チェスボードのコーナーの三次元座標(z=0)を、撮影画像枚数分入れる。
```
[OpenCV_CameraCalibration_L68 - GitHub](https://github.com/smk7758/OpenCV_CameraCalibration/blob/ee29c438ece9ff9646a2f4c0dd632da02425a76c/test-2018-12-10_OpenCV_CameraCalibration_2/src/com/github/smk7758/OpenCV_CameraCalibration/Main.java#L68)

getObjectPoints()の中身は以下のようになっています。

```java
	public List<Mat> getObjectPoints(int size, Size patternSize) {
		List<Mat> objectPoints = new ArrayList<>();
		for (int i = 0; i < size; i++) {
			objectPoints.add(getObjectPoint(patternSize));
		}
		return objectPoints;
	}

	public MatOfPoint3f getObjectPoint(Size patternSize) {
		MatOfPoint3f objectPoint = new MatOfPoint3f();

		List<Point3> objectPoint_ = new ArrayList<>();
		// final Size patternSize = new Size(6, 9); // 探査するコーナーの数
		for (int row = 0; row < patternSize.height; row++) {
			for (int col = 0; col < patternSize.width; col++) {
				objectPoint_.add(getPoint(row, col));
			}
		}

		objectPoint.fromList(objectPoint_);
		return objectPoint;
	}

	public Point3 getPoint(int row, int col) {
		final double REAL_HEIGHT = 20.0, REAL_WIDTH = 20.0;
		return new Point3(col * REAL_WIDTH, row * REAL_HEIGHT, 0.0); // 多分x, y, zはこういう感じ。
	}
```
[OpenCV_CameraCalibration_L96 - GitHub](https://github.com/smk7758/OpenCV_CameraCalibration/blob/ee29c438ece9ff9646a2f4c0dd632da02425a76c/test-2018-12-10_OpenCV_CameraCalibration_2/src/com/github/smk7758/OpenCV_CameraCalibration/Main.java#L96)

###画像上のチェストボードの格子点の座標を求める
自分で、findChessboardCorners()というメソッドを書きました。呼び出しは以下のようになっています。

```java
		List<Mat> outputFindChessboardCorners = new ArrayList<>();
		try (DirectoryStream<Path> ds = Files.newDirectoryStream(picFolderPath)) {
			for (Path path : ds) {
				System.out.println(path.toString());

				final Optional<Mat> outputMat = findChessboardCorners(path.toString(), imagePoints, patternSize);

				if (outputMat.isPresent()) {
					outputFindChessboardCorners.add(outputMat.get());
					System.out.println("successful to find corners.");
				} else {
					System.err.println("unsuccessful to find corners.");
				}
			}
		} catch (IOException ex) {
			ex.printStackTrace();
		}
```
[OpenCV_CameraCalibration_L50 - GitHub](https://github.com/smk7758/OpenCV_CameraCalibration/blob/ee29c438ece9ff9646a2f4c0dd632da02425a76c/test-2018-12-10_OpenCV_CameraCalibration_2/src/com/github/smk7758/OpenCV_CameraCalibration/Main.java#L50)

findChessboardCorners()の中身は以下のようになっています。
Calib3d.findChessboardCorners()というメソッドを利用しました。
なお、返却しているMatは、Calib3d.findChessboardCorners()が正常に実行できた画像が入っています。


```java
	public Optional<Mat> findChessboardCorners(String picPathString, List<Mat> imagePoints, Size patternSize) {
		Mat inputMat = Imgcodecs.imread(picPathString);
		Mat mat = inputMat.clone();
		// final Size patternSize = new Size(6, 9); // 探査するコーナーの数
		MatOfPoint2f corners = new MatOfPoint2f(); // in, 検出されたコーナーの二次元座標のベクトルを受け取る。

		Imgproc.cvtColor(inputMat, inputMat, Imgproc.COLOR_BGR2GRAY);

		final boolean canFindChessboard = Calib3d.findChessboardCorners(inputMat, patternSize, corners);

		if (!canFindChessboard) {
			System.err.println("Cannot find Chessboard Corners.");
			return Optional.empty();
		}

		imagePoints.add(corners);

		Calib3d.drawChessboardCorners(mat, patternSize, corners, true);

		Path picPath = Paths.get(picPathString);
		Path folderPath = Paths.get("S:\\CameraCaliblation\\2018-12-31_output");
		Path path = Paths.get(folderPath.toString(), picPath.getFileName().toString());

		if (!Files.exists(folderPath) || !Files.isDirectory(folderPath)) {
			try {
				System.out.println("There was no folder, so it is createing a folder. : " + folderPath.toString());
				Files.createDirectory(folderPath);
			} catch (IOException ex) {
				ex.printStackTrace();
			}
		}

		Imgcodecs.imwrite(path.toString(), mat);

		return Optional.of(inputMat);
	}
```

###Calib3d.calibrateCamera()を実行する
先程用意した空間上の格子点の座標と、先程求めた画像上のチェストボードの格子点の座標を引数として、[Calib3d.calibrateCamera()](http://opencv.jp/opencv-2svn/cpp/camera_calibration_and_3d_reconstruction.html#cv-calibratecamera)を実行します。
また、このメソッドでカメラ行列とレンズ歪み係数の値が返されるので、それを格納する変数を作っておきます。

```java
		// 受け取るもの
		Mat cameraMatrix = new Mat(), distortionCoefficients = new Mat();
		List<Mat> rotationMatrixs = new ArrayList<>(), translationVectors = new ArrayList<>();

		Calib3d.calibrateCamera(objectPoints, imagePoints, imageSize,
				cameraMatrix, distortionCoefficients, rotationMatrixs, translationVectors);
```
[OpenCV_CameraCalibration_L81 - GitHub](https://github.com/smk7758/OpenCV_CameraCalibration/blob/ee29c438ece9ff9646a2f4c0dd632da02425a76c/test-2018-12-10_OpenCV_CameraCalibration_2/src/com/github/smk7758/OpenCV_CameraCalibration/Main.java#L81)

###返された カメラ行列とレンズ歪み係数を保存する
カメラ行列とレンズ歪み係数を取得できましたが、この処理を毎回行うのは面倒かつ無駄なので、行列の値を保存しましょう。
CやPythonならはOpenCV側でMatの値を入力・出力するものがあるのですが、Java版ではないので、自分で今回は作りました。
Javaの標準ライブラリにあるXMLの入出力を使いました。

```java
		Map<String, Mat> exportMats = new HashMap<>();
		exportMats.put("CameraMatrix", cameraMatrix);
		exportMats.put("DistortionCoefficients", distortionCoefficients);
		final Path exportFilePath = Paths.get("S:\\CameraCaliblation\\CameraCalibration_2018-12-31.xml");
		MatIO.exportMat(exportMats, exportFilePath);
```

[OpenCV_CameraCalibration_L87 - GitHub](https://github.com/smk7758/OpenCV_CameraCalibration/blob/ee29c438ece9ff9646a2f4c0dd632da02425a76c/test-2018-12-10_OpenCV_CameraCalibration_2/src/com/github/smk7758/OpenCV_CameraCalibration/Main.java#L87)

MatIOの中身は、[OpenCV_CameraCalibration_MatIO.java - GitHub](https://github.com/smk7758/OpenCV_CameraCalibration/blob/master/test-2018-12-10_OpenCV_CameraCalibration_2/src/com/github/smk7758/OpenCV_CameraCalibration/MatIO.java) を参照してください。


##おわりに
一応、これを使って、[ArUcoのARマーカーの姿勢推定](https://qiita.com/smk7758/items/1ca1370f78cad1233d02#%E3%82%AB%E3%83%A1%E3%83%A9%E3%82%92%E7%94%A8%E3%81%84%E3%81%9F%E3%83%AA%E3%82%A2%E3%83%AB%E3%82%BF%E3%82%A4%E3%83%A0marker%E5%A7%BF%E5%8B%A2%E6%8E%A8%E5%AE%9A) を行ったところ、正常に動作しました。
我が部の情報班の後輩にも理解&実行してもらい、成功しましたので良かったです。

###参考リンク
・[OpenCV - カメラキャリブレーションを行う方法 - Pynote](http://pynote.hatenablog.com/entry/opencv-camera-calibration)
・[カメラキャリブレーション (OpenCV 1.0) - OpenCV.jp](http://opencv.jp/sample/camera_calibration.html)
・[OpenCVとVisual C++による画像処理と認識](http://ishidate.my.coocan.jp/opencv_17/opencv_17.htm)
・[pythonとOpenCVでカメラキャリブレーション（1個のカメラの内部パラメータと歪み係数を求める）するコード（パクリ）](http://russeng.hatenablog.jp/entry/2015/06/16/004704)
・[カメラ校正](https://qiita.com/nonbiri15/items/5e59dea66d7733d6d6a3)
・[OpenCV 2.4.11&C++によるカメラキャリブレーション - 技術的特異点](http://tecsingularity.com/opencv/cameracalib/)
・[Camera Calibration and 3D Reconstruction - OpenCV公式](https://docs.opencv.org/3.4.2/d9/d0c/group__calib3d.html)- 改めて公式サイト見ると良い説明ありますね。
・[CC_Controller.java - opencv-java/camera-calibration(GitHub)](https://github.com/opencv/opencv/blob/master/samples/android/camera-calibration/src/org/opencv/samples/cameracalibration/CameraCalibrator.java) - Java版のやり方に参考になりました。
・[cameracalibration - opencv(GitHub)](https://github.com/opencv/opencv/tree/master/samples/android/camera-calibration/src/org/opencv/samples/cameracalibration)