import numpy as np
import cv2
import sys
import re
import os
import commands
def menu():
	print """Use: python Calibracion_Fid_Chesseboard.py -p <path> -a <a> -b <b> -c <c> -t <format>
	[-p <path>]		# Directorio donde se encuentras las imagenes]
	[-a <a>			# Esquinas interiores verticales]
	[-b <b>			# Esquinas interiores horizontales]
	[-c <c>			# Dimension del cuadrado (1 lado)]
	[-t <format>		# Formato de las imagenes]"""

test=False
if len(sys.argv)<4:
	print "\nFaltan parametros..."
	menu()
	exit()
else:
	
	for i in range(1,len(sys.argv)):
		
		if sys.argv[i]=="-p":
			path=sys.argv[i+1]
			test=os.access(path,os.F_OK)
			
		if sys.argv[i]=="-a":
			a=int(sys.argv[i+1])
		if sys.argv[i]=="-b":
			l=int(sys.argv[i+1])
		if sys.argv[i]=="-t":
			formato=sys.argv[i+1]
		if sys.argv[i]=="-c":
			dimension=float(sys.argv[i+1])

if test==True:

	criteria=(cv2.TERM_CRITERIA_EPS+cv2.TERM_CRITERIA_MAX_ITER, 30,0.001)
	mtx=np.zeros((3,3),np.float32)
	dist=np.zeros((5,1),np.float32)
	objp=np.zeros((a*l,3),np.float32)
	objp[:,:2]=np.mgrid[0:a,0:l].T.reshape(-1,2)
	objp*=dimension
	objpoints=[]
	imgpoints=[]
	
	buscar="''ls "+path+"/*."+formato+"''"
	archivos=[str(x) for x in filter(None,re.split('[\n]',commands.getoutput(buscar)))]
	
	

	for j in range(len(archivos)): 	
		print archivos[j]
		imagen=cv2.imread(archivos[j])
		gray=cv2.cvtColor(imagen,cv2.COLOR_BGR2GRAY)
		ret,corners=cv2.findChessboardCorners(gray,(a,l),None)
		if ret == True:
			print " ON"
			cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
			cv2.drawChessboardCorners(imagen, (a,l),corners,ret)
			objpoints.append(objp)
			imgpoints.append(corners)
		else:
			print " OFF"
          	
		cv2.imshow('mostrar',imagen)
		cv2.waitKey()
	cv2.destroyAllWindows()

	ret,mtx,dist,rvecs,tvecs=cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1],None,None)
	mean_error = 0

	for i in range(len(objpoints)):
		imgpoints2, _ = cv2.projectPoints(objpoints[i], rvecs[i], tvecs[i], mtx, dist)
		error = cv2.norm(imgpoints[i],imgpoints2, cv2.NORM_L2)/len(imgpoints2)
		mean_error += error

	tError=mean_error/len(objpoints)
	print "total error: ", tError
	print 'Matriz calibracion',mtx
	print 'Matriz Distorsion',dist

	np.save(path+"/mat_calib.npy",mtx)
	np.save(path+"/coef_dist.npy",dist)
	np.save(path+"/Error_total.npy",tError)				



	
else: 
	print "No existe el directorio"
	menu()
		
	
