import org.opencv.core.*;
import org.opencv.calib3d.*;
import org.opencv.highgui.*;
import org.opencv.features2d.*;
import org.opencv.utils.*;
import org.opencv.imgproc.*;
import org.opencv.video.*;
import java.util.*;
import java.io.*;

class RegistrationExample {

	
  static{ System.loadLibrary(Core.NATIVE_LIBRARY_NAME); }


	public static void main(String[] args) {
	
		Mat pointSet1;
		Mat pointSet2;
		Mat T;
		
		pointSet1 = new Mat(5, 3, CvType.CV_32F);
		pointSet2 = new Mat(5, 3, CvType.CV_32F);
		
		float[] pointCoords1 = {2.4f, 5.1f, 1.0f};
		float[] pointCoords2 = {3.4f, 6.1f, 1.0f};
		float[] pointCoords3 = {5.4f, 4.1f, 1.0f};
		float[] pointCoords4 = {8.4f, 3.1f, 1.0f};
		float[] pointCoords5 = {25.4f, 2.1f, 1.0f};
		
		
		pointSet1.put(0,0, pointCoords1 );
		pointSet1.put(1,0, pointCoords2 );
		pointSet1.put(2,0, pointCoords3 );
		pointSet1.put(3,0, pointCoords4 );
		pointSet1.put(4,0, pointCoords5 );

		
		T = new Mat(3,3, CvType.CV_32F);
		double angle_rad = (30.0 * Math.PI) / 180.0;
		float cos_angle_rad = (float) (Math.cos(angle_rad));
		float sin_angle_rad = (float) (Math.sin(angle_rad));
		
		float[] row1 = {cos_angle_rad, -sin_angle_rad, 10.0f};
		float[] row2 = {sin_angle_rad, cos_angle_rad, 0.0f};
		//float[] row1 = {1.0f, 0.0f, 10.0f};
		//float[] row2 = {0.0f, 1.0f, 0.0f};
		float[] row3 = {0.0f, 0.0f, 1.0f};
		T.put(0,0, row1);
		T.put(1,0, row2);
		T.put(2,0, row3);
		
		System.out.println(T.dump());
		System.out.println(pointSet1.dump());
		
		Core.gemm( T, pointSet1.t(), 1, Mat.eye(2,2, CvType.CV_32F), 0.0,  pointSet2 );
		
		
		//System.out.println(pointSet2.dump());
		Mat points1 = new Mat();
		Mat points2 = new Mat();
		Calib3d.convertPointsFromHomogeneous(pointSet1, points1);
		
		Calib3d.convertPointsFromHomogeneous(pointSet2.t(), points2);
		
		System.out.println("\n-------------------\npoint set1");
		System.out.println(points1.dump());
		System.out.println("\n-------------------\npoint set2");
		System.out.println(points2.dump());
		
		
		Mat estimatedAffineTransform = Calib3d.findHomography(new MatOfPoint2f(points2), new MatOfPoint2f(points1));
		
		System.out.println("\n-------------------\ntransformation matrix:");
		System.out.println(estimatedAffineTransform.dump());
		Mat estimatedAffineTransform_F = new Mat();
		estimatedAffineTransform.convertTo(estimatedAffineTransform_F, CvType.CV_32F);
		
		Mat t_pointSet1 = new Mat();
		
		Core.gemm( estimatedAffineTransform_F.inv(), pointSet1.t(), 1, Mat.eye(3,3, CvType.CV_32F), 0.0,  t_pointSet1 );
		
		System.out.println("\n-------------------\ntransformed point set1 (must be transposed)");
		System.out.println(t_pointSet1.dump());
		
		Mat img = new Mat(40,40, CvType.CV_8U);
		Imgproc.cvtColor(img, img, Imgproc.COLOR_GRAY2BGR);
		img.setTo(new Scalar(0, 0, 0));
		
		
		List<Point> pointList1 = (new MatOfPoint2f(points1)).toList();
		List<Point> pointList2 = (new MatOfPoint2f(points2)).toList();
		
		Mat temp1 = new Mat();
		Calib3d.convertPointsFromHomogeneous(t_pointSet1.t(), temp1);
		MatOfPoint2f t_points1 = new MatOfPoint2f(temp1);
		//System.out.println(t_points2.dump());
		
		List<Point> t_pointList1 = t_points1.toList();
				
		for (int i = 0; i < pointList1.size()-1; i++) {
			Core.line(img, pointList1.get(i), pointList1.get(i+1), 
					    new Scalar(255.0, 0, 0) );  // point set1 -- blue
			Core.line(img, pointList2.get(i), pointList2.get(i+1), 
						new Scalar(0, 255, 0) ); // point set2 -- green
			Core.line(img, t_pointList1.get(i), t_pointList1.get(i+1), 
						new Scalar(0, 0, 255) ); // transformed point set1 -- red
		}
		
		Highgui.imwrite( "line_registration.png", img);
		
	}

}