#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~#
# 3D STEREO CALIBRATION TUTORIAL              #
#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~#
#
#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~#
# Pedro Veras Guimarães
# Ifremer, 19/05/2015
#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~#

#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~#
# 1st - Prepare images and directories        #
#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~#

# Define Calib Tool directory and Run Tool directory
CTOOL='/home/cercache/project/stereo/TOOLS/SCRIPTS/CALIB'
RUNDIR='/home/cercache/project/stereo/PORSMILIN_201504'

# Create a directory for Calib images:
>> mkdir -p Calib/
>> mkdir -p Calib/IMAGES/
>> cd Calib/

# Separate the images for Intrinsic and Extrinsic calibration

# Copy or move the images to this directory
>> cp run_????_*_????_intr IMAGES/run_????_*_????_intr
>> cp run_????_*_????_extr IMAGES/run_????_*_????_extr

# Verify and filter the images:
>> cd ${RUNDIR}/IMAGES/run_????_*_????_intr
>> ${CDIR}/./select_intr.sh IMAGES/run_????_*_????_intr/

>> cd ${RUNDIR}/IMAGES/run_????_*_????_extr
>> ${CDIR}/./select_extr.sh IMAGES/run_????_*_????_extr/

# Rename and move to IMAGES/LAFT and RIGTH
>> cd ${RUNDIR}/IMAGES/run_????_*_????_intr
>> ${CDIR}/./rename_intr.sh IMAGES/run_????_*_????_intr/

>> cd ${RUNDIR}/IMAGES/run_????_*_????_extr
>> ${CDIR}/./rename_extr.sh IMAGES/run_????_*_????_extr/
* it will numerate the images for intrisic calibration from 0 to aproximately 200 and the extrinsic images from 1000 to

#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~#
# 2nd - Identify the chassboard in the images #
#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~#
# Usage of calib_gui on MatLab

# open matlab
>> matlab

# add calib rotunes to matlab path

>> calib_gui
    -> chose memory eficient mode

# Go to RIGHT/ and LEFT/ folder.
>> cd CALIB/LEFT or CALIB/RIGHT
      # in calib_gui select
      -> Image names
          -> Use the sufix as Left_ or Right_
          -> Type t fot ".tif" format


      # in calib_gui select
      -> Extract grid corneres
          -> Type [] for all images
          -> define dx and dy of the chessboard = 50mm and 50mm     for intrisic images and 100mm for extrincic images
          -> Options:
                      p => Modify parameters (dX,dY,...), use it when the images chage from 0 to 1000 (intr to extr)
                      s => Skip images (desactivate image for calibration), use it if the identification is not good or the chessboard is not in the image.
              obs: do not type the option in the matlab terminal, do not use captal letters, use this option when the figure window are selected.
          -> Clik inside of the chessboard in the image.
          -> Just press enter if it is good or type somethig if is not good.

# Go to step 3 and after repeat the same process for RIGHT and LEFT folder.

#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~#
# 3nd - Make the INTR Calibration  #
#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~#
# Go inside of RIGHT and LEFT folder.
# Using calib_gui on MatLab go to "Calibration"
* look the error, the pixel error should be smaller than 0.2

# Using calib_gui on MatLab "Save"

# Correct the error:
# Using calib_gui on MatLab "Analyse error"
  -> use maxerr => Select max error = 2
  -> Using calib_gui select "Add/Suppress images"
      -> type 0 for suppress
      -> FL_LISTE  will supreess the images with more thant 2 pixels error
  -> Run "Calibration" again and than "Save"

# Check the Calibration result
  -> Using calib_gui select "Reproject on images"
  it will reproject the calibration on image, it is good check one by one.

# Repeat the same process for RIGHT and LEFT folder.

#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~#
# 4nd - Verify the INTR Calibration results  #
#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~#
# Go inside of RIGHT and LEFT folder.
# Using calib_gui on MatLab go to "Calibration"
* look the error, the pixel error should be smaller than 0.2

# Using calib_gui on MatLab "Reproject on images"
-> to show the 1th 50 images: 20:-1:1, ... 40:-1:20, ...

take a note of the bad chess board conections in the images. The chess board should be like this:
      Y
 _ _ _ _ _ _ 0
|_|_|_|_|_|_|
|_|_|_|_|_|_|
|_|_|_|_|_|_| X
|_|_|_|_|_|_|
|_|_|_|_|_|_|

with 90^o between X and Y coordinates.

# Supress those bad images from calibration
Using calib_gui on MatLab "Add/Suppress images"
>> 0
>> [1 3 17 129 ... N] # include your note list of images to suppress

# Run the "Calibration" and "Analyse erro" again
>> 2

# "Save"

# Repeat the same process for RIGHT and LEFT folder.

#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~#
# 5nd - Run Extrinsic Calibration            #
#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~#
# on CALIB/ folder Create STEREO dir
>> !mkdir STEREO
>> cd STEREO

# copy right and left intrinsic calibration
>> !cp ../LEFT/Calib_Results.mat Calib_Results_left.mat
>> !cp ../RIGHT/Calib_Results.mat Calib_Results_right.mat

# Load intrinsic calibration
# Using stereo_gui menu on MatLab "Load left and right calibration files"
>> Calib_Results_left.mat
>> Calib_Results_right.mat

# Run stereo calibration
Using stereo_gui menu on MatLab "Run stereo calibration"

# Verify results
 "Show Extrinsics of stereo rig"
 "Show Intrinsic parameters"

# Save results
"Save stereo calib results"

#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~#
# 6th - Ends Calibration            #
#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~#
# copy the results to Calib folder
>> cd ../../../Calib/
>> !cp CALIB/STEREO/*.mat .

# Comput the calibration results:
# on matlab terminal
>> Create_Calib_Index_Files
    >> 1
    >> 2
    >> Calib_Results_left.mat
    >> Calib_Results_right.mat
    >> Calib_Results_stereo.mat

#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~#
# Optional - Add more images for calibration  #
#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~#
# Using calib_gui on matlabe
# on RIGHT or LEFT folder, "Load" calib results

# Show extrinsec (affiche les damiers déjà traités)

# Read images (actualise les images présentes dans le dossier courant)

# Extrat grid corner
>> selection des images a retraiter
>> active_images(:) = 1;

#"Calibration"
#"Analyse error" (renvoie FL_LISTE)

# "Recomp corner" (si necessaire)
>> FL_LISTE

# "Calibration"

# "Analyse_error" (renvoie FL_LISTE)


# "Save"