#------------------------------------------------------
ZHU Weixu
	zhuweixu_harry@126.com

updated by 10 Oct 2018, fixed some problems after real robot test
#------------------------------------------------------


This is a simple testbench for calculating the 3D pose of apriltags from its 2D projection in
the images.
the algorithm is described in detail in the .pdf file
https://github.com/freedomcondor/testTag/blob/master/solution-april-tag.pdf

we tested this on ubuntu 16.04 LTS, with lua5.2 and OpenCV 3.0.0, 
   other versions are untested, you can have a try
#####################################################################################
Brief Introduction

to run this test, you should have these libraries
	1. Lua
	2. OpenCV

	If you do not have lua, sudo apt-get install lua5.2 should work
	if you do not have OpenCV, see:
	https://docs.opencv.org/3.0-beta/doc/tutorials/introduction/linux_install/linux_install.html

The structure of the code is like this:

main.cpp   <----|  testbench  <----- lua

main.cpp calls init(), step() and close() functions in testbench

the testbench loads images one by one from images in data sets
		  and loads apriltags data sets of 2D coordinates from .dat file
	      and then call lua to calculate the pos of the tags.
		  After calling lua, it retrieve the results and draw them on images.

######################################################################################

Folders and Files

There are 3 folders : testbench, lua, data

1. data folder a data set, which a video taken by a robot
   there are: 
		a folder, containing all the images of a video,
	 	a .txt file, containing the names of all the images
	 	a .dat file, containing the 2D information of all apriltags in every image 
			information includes: the center and corner locations of the 2D positions in the 
			image as a .dat file so that you do not need an apriltag library installed to detect 
			tags when running this testbench.

2. testbench: There are main.cpp, testbench.h and testbench.cpp in testbench folder.
	main.cpp includes testbench.h, and testbench calls lua

3. lua: There are a lot of things in lua folder 
	testbench functions will call a function named "func" in func.lua 
	and func.lua will call calcPos.lua and tracking.lua

	"calTagPos" function in calcPos.lua will call solveSqu/solveSquare.lua, which is the core 
	algorithm which calc the 3D location of a tag from its 2D projection in the image.

	In lua folder, additional to lua, there is also a solvepnp folder containing a lua wrapper on 
	opencv's solvepnp function so that opencv's method can be compared. 

	tracking.lua is not relevant here, it is used to track blocks after locating.

#####################################################################################

Compile and run

  Compile:
	Go to root folder, where there is a CMakeLists, and do typical cmake-make thing:
	Create a build folder, go inside and run cmake to generate a Makefile, and make, this will
	generate a executable file:
		
	commands:
		testTag$ mkdir build
		testTag$ cd build
		testTag/build$ cmake ..
		testTag/build$ make
  Run:
  	To run, simply run the executable file:
		./luaTag

3. OpenCV
    If you want to use solvepnp function in opencv to compare, go to folder lua/solvepnp, 
	build, cmake and make, generate a libsolvepnp.so in the build folder. It will be used by lua.
    
###################################################################################
SolveSqu:

The core function (the solveSquare(), who calculate the position and rotation of a tag) is located in 
	lua/solveSqu/solveSqu.lua

###################################################################################