#!/usr/bin/perl
# -*- mode: perl; coding: utf-8; tab-width: 4; -*-

# http://opencv.jp/sample/camera_calibration.html

use strict;
use warnings;
use lib qw(blib/lib blib/arch);
use Cv;
use IO::File;
use File::Basename;

my @images;
my $calib_image;

&Calibrate($ARGV[0] || join('/', dirname($0), "calibration.txt"));
&Undistort($calib_image);
# &UndistortMap($calib_image);
exit 0;

sub Calibrate {
	# (1) キャリブレーション画像の読み込み
	my $imageList = shift;
	my $f = new IO::File $imageList, "r";
	die "can not open file $imageList\n" unless ($f);

	while (<$f>) {
		next if (/^#/);
		chomp;
		my $image;
		if (-f $_) {
			$image = Cv->LoadImage($_, CV_LOAD_IMAGE_COLOR);
		} else {
			$image = Cv->LoadImage(join('/', dirname($0), $_),
								   CV_LOAD_IMAGE_COLOR);
		}
		die "cannot load image file : $_\n" unless $image;
		push(@images, $image);
		$calib_image = $image->clone unless $calib_image;
	}
	
	# (2) 3次元空間座標の設定
	my $IMAGE_NUM = @images;		# 画像数
	my $PAT_ROW = 6;				# パターンの行数
	my $PAT_COL = 9;				# パターンの列数
	my $PAT_SIZE   = $PAT_ROW * $PAT_COL;
	my $ALL_POINTS = $IMAGE_NUM * $PAT_SIZE;
	my $CHESS_SIZE = 24.0;			# パターン1マスの1辺サイズ[mm]
	
	my $object_points = Cv::Mat->new([$ALL_POINTS, 1], CV_32FC3);
#	my $object_points = Cv::Mat->new([$ALL_POINTS, 3], CV_32FC1);

	for my $i (0 .. $IMAGE_NUM-1) {
		for my $j (0 .. $PAT_ROW-1) {
			for my $k (0 .. $PAT_COL-1) {
				$object_points->set(
					[$i*$PAT_SIZE + $j*$PAT_COL + $k, 0],
					[$j*$CHESS_SIZE, $k*$CHESS_SIZE, 0],
					);
			}
		}
	}

	# (3) チェスボード（キャリブレーションパターン）のコーナー検出
	my $image_points = Cv::Mat->new([$ALL_POINTS, 1], CV_32FC2); # corners
	my $point_counts = Cv::Mat->new([$IMAGE_NUM,  1], CV_32SC1); # p_count;
	
	my $pattern_size = [$PAT_COL, $PAT_ROW];
	my $found_num = 0;

	for (my $i = 0; $i < $IMAGE_NUM; $i++) {
		my $found = $images[$i]->FindChessboardCorners(
			$pattern_size, \ my @corners,
			&CV_CALIB_CB_ADAPTIVE_THRESH | &CV_CALIB_CB_NORMALIZE_IMAGE,
			);

		printf STDERR "%02d...", $i;
		if ($found) {
			print STDERR "ok\n";
			$found_num++;
		} else {
			print STDERR "fail\n";
		}

		# (4) コーナー位置をサブピクセル精度に修正，描画
		my $gray = $images[$i]->CvtColor(CV_BGR2GRAY);
		$gray->FindCornerSubPix(
			\@corners, [3, 3], [-1, -1], cvTermCriteria(
				CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03),
			);
		$images[$i]->DrawChessboardCorners(
			$pattern_size, \@corners, $found);
		$point_counts->set([$i, 0], [scalar @corners]);
		for (0 .. $#corners) {
			$image_points->set([$i*$PAT_SIZE + $_, 0], $corners[$_]);
		}
		Cv->NamedWindow("Calibration", 0);
		$images[$i]->ShowImage("Calibration");
		Cv->WaitKey(100);
	}
	exit -1 if ($found_num != $IMAGE_NUM);

	# (5) 内部パラメータ，歪み係数の推定
	my $intrinsic   = Cv::Mat->new([3, 3], CV_32FC1);
	my $distortion  = Cv::Mat->new([1, 4], CV_32FC1);

	my $rvects = Cv::Mat->new([$IMAGE_NUM, 3], CV_64FC1);
	my $tvects = Cv::Mat->new([$IMAGE_NUM, 3], CV_64FC1);

	$object_points->CalibrateCamera2(
		$image_points, $point_counts, $calib_image->size,
		$intrinsic, $distortion, $rvects, $tvects,
		);

	# (6) 外部パラメータの推定
	my $sub_image_points = Cv::Mat->new([$PAT_SIZE, 1], CV_32FC2);
#	my $sub_object_points = Cv::Mat->new([$PAT_SIZE, 1], CV_32FC3);
	my $sub_object_points = Cv::Mat->new([$PAT_SIZE, 3], CV_32FC1);
	my $base = 0;
	$image_points->GetRows(
		$sub_image_points, $base * $PAT_SIZE, ($base+1) * $PAT_SIZE,
		);
	$object_points->GetRows(
		$sub_object_points, $base * $PAT_SIZE, ($base+1) * $PAT_SIZE,
		);
	my $rotation    = Cv::Mat->new([1, 3], CV_32FC1);
	my $translation = Cv::Mat->new([1, 3], CV_32FC1);
	$sub_object_points->FindExtrinsicCameraParams2(
		$sub_image_points, $intrinsic, $distortion,
		$rotation, $translation,
		);

	# Cv->Save("intrinsic.xml", $intrinsic);
	# Cv->Save("distortion.xml", $distortion);

	# (7) XMLファイルへの書き出し
	my $fs = Cv::FileStorage->new("camera.xml", CV_STORAGE_WRITE);
	$fs->Write("intrinsic", $intrinsic);
	$fs->Write("rotation", $rotation);
	$fs->Write("translation", $translation);
	$fs->Write("distortion", $distortion);
}


sub Undistort {
	# (1)補正対象となる画像の読み込み
	my $src_img = shift;
	$src_img = $src_img->CvtColor(CV_RGB2GRAY) if ($src_img->channels == 3);

	# (2)パラメータファイルの読み込み
	my $param;
	my $fs = Cv::FileStorage->new("camera.xml", CV_STORAGE_READ);
	$param = $fs->GetFileNodeByName(\0, "intrinsic");
	my $intrinsic = $fs->Read($param);
	$param = $fs->GetFileNodeByName(\0, "distortion");
	my $distortion = $fs->Read($param);

	# (3)歪み補正
	$src_img->Undistort2(my $dst_img = $src_img->new, $intrinsic, $distortion);

	# (4)画像を表示，キーが押されたときに終了
	Cv->NamedWindow("Distortion", 0);
	Cv->NamedWindow("UnDistortion", 0);
	$src_img->ShowImage("Distortion");
	$dst_img->ShowImage("UnDistortion");
	Cv->WaitKey;
}


sub UndistortMap {
	# (1)補正対象となる画像の読み込み
	my $src_img = shift;
	$src_img = $src_img->CvtColor(CV_RGB2GRAY) if ($src_img->channels == 3);
	my $mapx = Cv::Image->new([480, 640], CV_32FC1);
	my $mapy = Cv::Image->new([480, 640], CV_32FC1);

	# (2)パラメータファイルの読み込み
	my $param;
	my $fs = Cv::FileStorage->new("camera.xml", CV_STORAGE_READ);
	$param = $fs->GetFileNodeByName(\0, "intrinsic");
	my $intrinsic = $fs->Read($param);
	$param = $fs->GetFileNodeByName(\0, "distortion");
	my $distortion = $fs->Read($param);

	# (3) 歪み補正のためのマップ初期化
	$intrinsic->InitUndistortMap($distortion, $mapx, $mapy);

	# (4) 歪み補正
	$src_img->Remap(my $dst_img = $src_img->new, $mapx, $mapy);

	Cv->NamedWindow("Distortion", 0);
	Cv->NamedWindow("UnDistortion", 0);
	$src_img->ShowImage("Distortion");
	$dst_img->ShowImage("UnDistortion");
	Cv->WaitKey;
}