diff --git a/Assets/ArucoUnity/CameraParameters/Logitech HD Webcam C270 - 2021-03-09_15-59-08.xml b/Assets/ArucoUnity/CameraParameters/old/Logitech HD Webcam C270 - 2021-03-09_15-59-08.xml
similarity index 100%
rename from Assets/ArucoUnity/CameraParameters/Logitech HD Webcam C270 - 2021-03-09_15-59-08.xml
rename to Assets/ArucoUnity/CameraParameters/old/Logitech HD Webcam C270 - 2021-03-09_15-59-08.xml
diff --git a/Assets/ArucoUnity/CameraParameters/Logitech HD Webcam C270 - 2021-03-09_15-59-08.xml.meta b/Assets/ArucoUnity/CameraParameters/old/Logitech HD Webcam C270 - 2021-03-09_15-59-08.xml.meta
similarity index 100%
rename from Assets/ArucoUnity/CameraParameters/Logitech HD Webcam C270 - 2021-03-09_15-59-08.xml.meta
rename to Assets/ArucoUnity/CameraParameters/old/Logitech HD Webcam C270 - 2021-03-09_15-59-08.xml.meta
diff --git a/Assets/ArucoUnity/CameraParameters/old/Logitech HD Webcam C270 - 2021-03-12_11-38-28.xml b/Assets/ArucoUnity/CameraParameters/old/Logitech HD Webcam C270 - 2021-03-12_11-38-28.xml
new file mode 100644
index 0000000000000000000000000000000000000000..667c64ebf91fde2a816f9f35f6d6bcdd81b61b87
--- /dev/null
+++ b/Assets/ArucoUnity/CameraParameters/old/Logitech HD Webcam C270 - 2021-03-12_11-38-28.xml	
@@ -0,0 +1,52 @@
+<?xml version="1.0" encoding="utf-8"?>
+<ArucoCameraParameters xmlns:xsd="http://www.w3.org/2001/XMLSchema" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance">
+  <CalibrationDateTime>2021-03-12T11:38:28.5941946+01:00</CalibrationDateTime>
+  <CameraNumber>1</CameraNumber>
+  <ImageHeights>
+    <int>960</int>
+  </ImageHeights>
+  <ImageWidths>
+    <int>1280</int>
+  </ImageWidths>
+  <CalibrationFlagsValue>256</CalibrationFlagsValue>
+  <FixAspectRatioValue>0</FixAspectRatioValue>
+  <ReprojectionErrors>
+    <double>0.34525471497848387</double>
+  </ReprojectionErrors>
+  <CameraMatricesType>CV_64F</CameraMatricesType>
+  <CameraMatricesValues>
+    <ArrayOfArrayOfDouble>
+      <ArrayOfDouble>
+        <double>1565.6836213599649</double>
+        <double>0</double>
+        <double>655.13551348748661</double>
+      </ArrayOfDouble>
+      <ArrayOfDouble>
+        <double>0</double>
+        <double>1561.0532316768945</double>
+        <double>498.98571528189655</double>
+      </ArrayOfDouble>
+      <ArrayOfDouble>
+        <double>0</double>
+        <double>0</double>
+        <double>1</double>
+      </ArrayOfDouble>
+    </ArrayOfArrayOfDouble>
+  </CameraMatricesValues>
+  <DistCoeffsType>CV_64F</DistCoeffsType>
+  <DistCoeffsValues>
+    <ArrayOfArrayOfDouble>
+      <ArrayOfDouble>
+        <double>-0.066501567635011333</double>
+        <double>1.7745956695721687</double>
+        <double>-0.0059086289790881335</double>
+        <double>0.00034044334409837556</double>
+        <double>-6.8314016037135</double>
+      </ArrayOfDouble>
+    </ArrayOfArrayOfDouble>
+  </DistCoeffsValues>
+  <OmnidirXisType>CV_8U</OmnidirXisType>
+  <OmnidirXisValues>
+    <ArrayOfArrayOfDouble />
+  </OmnidirXisValues>
+</ArucoCameraParameters>
\ No newline at end of file
diff --git a/Assets/ArucoUnity/CameraParameters/old/Logitech HD Webcam C270 - 2021-03-12_11-38-28.xml.meta b/Assets/ArucoUnity/CameraParameters/old/Logitech HD Webcam C270 - 2021-03-12_11-38-28.xml.meta
new file mode 100644
index 0000000000000000000000000000000000000000..2412c4d43d8654e093b5aba4b082fbfa988427c2
--- /dev/null
+++ b/Assets/ArucoUnity/CameraParameters/old/Logitech HD Webcam C270 - 2021-03-12_11-38-28.xml.meta	
@@ -0,0 +1,7 @@
+fileFormatVersion: 2
+guid: ec16c26c1758f16469a80ac5d777f7af
+TextScriptImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 
diff --git a/Assets/ArucoUnity/CameraParameters/old/Logitech HD Webcam C270 - 2021-03-12_11-43-58.xml b/Assets/ArucoUnity/CameraParameters/old/Logitech HD Webcam C270 - 2021-03-12_11-43-58.xml
new file mode 100644
index 0000000000000000000000000000000000000000..a578b60dbfda60069883891372508a740c5de562
--- /dev/null
+++ b/Assets/ArucoUnity/CameraParameters/old/Logitech HD Webcam C270 - 2021-03-12_11-43-58.xml	
@@ -0,0 +1,52 @@
+<?xml version="1.0" encoding="utf-8"?>
+<ArucoCameraParameters xmlns:xsd="http://www.w3.org/2001/XMLSchema" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance">
+  <CalibrationDateTime>2021-03-12T11:43:58.2587459+01:00</CalibrationDateTime>
+  <CameraNumber>1</CameraNumber>
+  <ImageHeights>
+    <int>960</int>
+  </ImageHeights>
+  <ImageWidths>
+    <int>1280</int>
+  </ImageWidths>
+  <CalibrationFlagsValue>256</CalibrationFlagsValue>
+  <FixAspectRatioValue>0</FixAspectRatioValue>
+  <ReprojectionErrors>
+    <double>0.44179991538397262</double>
+  </ReprojectionErrors>
+  <CameraMatricesType>CV_64F</CameraMatricesType>
+  <CameraMatricesValues>
+    <ArrayOfArrayOfDouble>
+      <ArrayOfDouble>
+        <double>1410.6455545674139</double>
+        <double>0</double>
+        <double>654.53381045256845</double>
+      </ArrayOfDouble>
+      <ArrayOfDouble>
+        <double>0</double>
+        <double>1407.7122598046378</double>
+        <double>501.22486451654828</double>
+      </ArrayOfDouble>
+      <ArrayOfDouble>
+        <double>0</double>
+        <double>0</double>
+        <double>1</double>
+      </ArrayOfDouble>
+    </ArrayOfArrayOfDouble>
+  </CameraMatricesValues>
+  <DistCoeffsType>CV_64F</DistCoeffsType>
+  <DistCoeffsValues>
+    <ArrayOfArrayOfDouble>
+      <ArrayOfDouble>
+        <double>-0.0708202278328505</double>
+        <double>1.306528079984052</double>
+        <double>0.00061926472615511327</double>
+        <double>0.0027684634873413098</double>
+        <double>-3.9332978302905888</double>
+      </ArrayOfDouble>
+    </ArrayOfArrayOfDouble>
+  </DistCoeffsValues>
+  <OmnidirXisType>CV_8U</OmnidirXisType>
+  <OmnidirXisValues>
+    <ArrayOfArrayOfDouble />
+  </OmnidirXisValues>
+</ArucoCameraParameters>
\ No newline at end of file
diff --git a/Assets/ArucoUnity/CameraParameters/old/Logitech HD Webcam C270 - 2021-03-12_11-43-58.xml.meta b/Assets/ArucoUnity/CameraParameters/old/Logitech HD Webcam C270 - 2021-03-12_11-43-58.xml.meta
new file mode 100644
index 0000000000000000000000000000000000000000..816090cfcc20070d07d19cacba217e5440f14a12
--- /dev/null
+++ b/Assets/ArucoUnity/CameraParameters/old/Logitech HD Webcam C270 - 2021-03-12_11-43-58.xml.meta	
@@ -0,0 +1,7 @@
+fileFormatVersion: 2
+guid: 9de06284d742fab42bdc1ab794f9fe4b
+TextScriptImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 
diff --git a/Assets/ArucoUnity/Scripts/Cameras/Displays/ArucoCameraDisplay.cs b/Assets/ArucoUnity/Scripts/Cameras/Displays/ArucoCameraDisplay.cs
index 6504677e0b3eaf59a7bda6d399c74cbaa8c39c6f..76a6d67f19bb0215e81fd18a41ad5f318efa09c9 100644
--- a/Assets/ArucoUnity/Scripts/Cameras/Displays/ArucoCameraDisplay.cs
+++ b/Assets/ArucoUnity/Scripts/Cameras/Displays/ArucoCameraDisplay.cs
@@ -154,8 +154,6 @@ namespace ArucoUnity.Cameras.Displays
       Vector2 cameraF = ArucoCameraUndistortion.RectifiedCameraMatrices[cameraId].GetCameraFocalLengths();
 
       float fovY = 2f * Mathf.Atan(0.5f * imageHeight / cameraF.y) * Mathf.Rad2Deg;
-            print(cameraF.y);
-            print(imageHeight);
       Cameras[cameraId].fieldOfView = fovY;
       BackgroundCameras[cameraId].fieldOfView = fovY;
     }
diff --git a/Assets/ForegroundTexture.cs b/Assets/ForegroundTexture.cs
deleted file mode 100644
index 5ec78da17e2a153fe6157e33c8dae6f20cc31518..0000000000000000000000000000000000000000
--- a/Assets/ForegroundTexture.cs
+++ /dev/null
@@ -1,36 +0,0 @@
-using System.Collections;
-using System.Collections.Generic;
-using UnityEngine;
-using OpenCvSharp;
-
-public class ForegroundTexture : MonoBehaviour
-{
-
-    private VideoCapture capture;
-    private Window window;
-    private Mat image;
-
-    // Start is called before the first frame update
-    void Start()
-    {
-        VideoCapture capture = new VideoCapture(0);
-        using (Window window = new Window("Camera"))
-        using (Mat image = new Mat()) // Frame image buffer
-        {
-            // When the movie playback reaches end, Mat.data becomes NULL.
-            while (true)
-            {
-                capture.Read(image); // same as cvQueryFrame
-                if (image.Empty()) break;
-                window.ShowImage(image);
-                Cv2.WaitKey(30);
-            }
-        }
-    }
-
-    // Update is called once per frame
-    void Update()
-    {
-
-    }
-}
diff --git a/Assets/ForegroundTexture.cs.meta b/Assets/ForegroundTexture.cs.meta
deleted file mode 100644
index 816d5411d51ef61238c06c925c451b2b1b03edf9..0000000000000000000000000000000000000000
--- a/Assets/ForegroundTexture.cs.meta
+++ /dev/null
@@ -1,11 +0,0 @@
-fileFormatVersion: 2
-guid: 25b24fb7ce8bb184c854f1de79b4d334
-MonoImporter:
-  externalObjects: {}
-  serializedVersion: 2
-  defaultReferences: []
-  executionOrder: 0
-  icon: {instanceID: 0}
-  userData: 
-  assetBundleName: 
-  assetBundleVariant: 
diff --git a/Assets/Scenes/Main.unity b/Assets/Scenes/Main.unity
index 1641f14259b6b51debdb5368d4f011571be6be43..73e4ce617730434e9aca5d36820c2e6999cebb6d 100644
--- a/Assets/Scenes/Main.unity
+++ b/Assets/Scenes/Main.unity
@@ -135,6 +135,18 @@ GameObject:
     type: 3}
   m_PrefabInstance: {fileID: 1819456834}
   m_PrefabAsset: {fileID: 0}
+--- !u!114 &35529236 stripped
+MonoBehaviour:
+  m_CorrespondingSourceObject: {fileID: 114522793529101426, guid: 74c9e29e9d222a4438669336303acc9a,
+    type: 3}
+  m_PrefabInstance: {fileID: 1819456834}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 35529235}
+  m_Enabled: 1
+  m_EditorHideFlags: 0
+  m_Script: {fileID: 11500000, guid: e044568ea72405e49bf56df52f9d2b57, type: 3}
+  m_Name: 
+  m_EditorClassIdentifier: 
 --- !u!1 &37991299
 GameObject:
   m_ObjectHideFlags: 0
@@ -1722,6 +1734,7 @@ MonoBehaviour:
   ForegroundDisplay: {fileID: 287174137}
   maxImagesBG: 10
   thresh: 30
+  arucoUndistortion: {fileID: 35529236}
 --- !u!64 &332210474
 MeshCollider:
   m_ObjectHideFlags: 0
@@ -2514,8 +2527,8 @@ RectTransform:
   m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
   m_AnchorMin: {x: 0, y: 1}
   m_AnchorMax: {x: 0, y: 1}
-  m_AnchoredPosition: {x: 180, y: -136}
-  m_SizeDelta: {x: 301, y: 200}
+  m_AnchoredPosition: {x: 180, y: -174.51}
+  m_SizeDelta: {x: 301, y: 306.49}
   m_Pivot: {x: 0.5, y: 0.5}
 --- !u!114 &1029942013
 MonoBehaviour:
@@ -2539,7 +2552,7 @@ MonoBehaviour:
       m_Calls: []
   m_FontData:
     m_Font: {fileID: 10102, guid: 0000000000000000e000000000000000, type: 0}
-    m_FontSize: 30
+    m_FontSize: 25
     m_FontStyle: 0
     m_BestFit: 0
     m_MinSize: 2
@@ -2550,8 +2563,8 @@ MonoBehaviour:
     m_HorizontalOverflow: 0
     m_VerticalOverflow: 0
     m_LineSpacing: 1
-  m_Text: "Webcam Calibration\n\nPositions tracked : 0\nDeviation : 0.0\xB0\n(R)
-    Reset"
+  m_Text: "Axis Calibration\n\nPositions tracked : 0\nDeviation : 0.0\xB0\n(R) Reset
+    All\n(W) Back to main scene\n(S) Switch to Axis calib."
 --- !u!222 &1029942014
 CanvasRenderer:
   m_ObjectHideFlags: 0
diff --git a/Assets/WebcamCalibration.cs b/Assets/WebcamCalibration.cs
index 81c86cde0d4aa9af601f000405cc3b32c70e6079..335ea1f5f298544d3403b66f537925e043a54228 100644
--- a/Assets/WebcamCalibration.cs
+++ b/Assets/WebcamCalibration.cs
@@ -21,15 +21,19 @@ public class WebcamCalibration : MonoBehaviour
     private ArucoCameraParametersController ArucoWebcamParamScript;
     private List<ConfigurableController> ArucoWebcamScripts;
     private bool doCalibrate;
+    private bool doCalibrateFOV;
     private List<Quaternion> calibrationData;
     private Quaternion calibratedRotation;
+    private List<float> FOVCorrectionData;
 
     // Start is called before the first frame update
     void Start()
     {
         calibratedRotation = Quaternion.Euler(new Vector3(0, 180f, 0));
         doCalibrate = false;
+        doCalibrateFOV = false;
         calibrationData = new List<Quaternion>();
+        FOVCorrectionData = new List<float>();
 
         ArucoWebcamScripts = new List<ConfigurableController>();
 
@@ -53,9 +57,6 @@ public class WebcamCalibration : MonoBehaviour
             return f2.CreationTime.CompareTo(f1.CreationTime);
         });
 
-        foreach (FileInfo file in files)
-            print(file.Name);
-
         if (files.Length > 0)
         {
             ArucoWebcamParamScript.CameraParametersFilename = files[0].Name;
@@ -82,8 +83,7 @@ public class WebcamCalibration : MonoBehaviour
         if (doCalibrate)
         {
             loadWebcamParams();
-            print(ArucoWebcamParamScript.CameraParameters);
-            InvokeRepeating("calibrate", 0.5f, 1f);
+            InvokeRepeating("calibrateAxis", 0.5f, 1f);
             ScreenWebcamScript.stopCam();
             foreach (ConfigurableController ARController in ArucoWebcamScripts)
             {
@@ -104,19 +104,18 @@ public class WebcamCalibration : MonoBehaviour
         }
     }
 
-    public void calibrate()
+    private void calibrateAxis()
     {
         //if marker spotted add a new value
         if (ARCalibrator.gameObject.activeSelf) {
             //update projection matrix (calibrated with aruco)
             WebcamDisplay.projectionMatrix = ARucoWebcam.projectionMatrix;
-            WebcamDisplay.fieldOfView = ARucoWebcam.fieldOfView;
 
             Quaternion ARWebcamAxis = new Quaternion();
-            ARWebcamAxis.SetLookRotation(ARCalibrator.position - WebcamDisplay.transform.position);
+            ARWebcamAxis.SetLookRotation(ARCalibrator.position - WebcamDisplay.transform.position, WebcamDisplay.transform.up);
 
             Quaternion OTWebcamAxis = new Quaternion();
-            OTWebcamAxis.SetLookRotation(OTCalibrator.position - WebcamDisplay.transform.position);
+            OTWebcamAxis.SetLookRotation(OTCalibrator.position - WebcamDisplay.transform.position, ARucoWebcam.transform.up);
 
             WebcamDisplay.transform.Rotate(OTWebcamAxis.eulerAngles - ARWebcamAxis.eulerAngles);
             ARucoWebcam.transform.Rotate(OTWebcamAxis.eulerAngles - ARWebcamAxis.eulerAngles);
@@ -132,7 +131,45 @@ public class WebcamCalibration : MonoBehaviour
         }
     }
 
-    public void updateRotationFromValues()
+    //calibrate vertical FOV from angles (correcting Aruco FOV)
+    private void calibrateFOV()
+    {
+        //if marker spotted add a new value
+        if (ARCalibrator.gameObject.activeSelf)
+        {
+            //get pos of marker in local coordinates of aruco camera 
+            Vector3 ARPosVert = ARucoWebcam.transform.InverseTransformPoint(ARCalibrator.position);
+            ARPosVert.x = 0; //ignore horizontal offset
+            float ARAngle = Mathf.Atan(ARPosVert.y / ARPosVert.z);
+
+            //same for OT marker
+            Vector3 OTPosVert = ARucoWebcam.transform.InverseTransformPoint(OTCalibrator.position);
+            OTPosVert.x = 0; //ignore horizontal offset
+            float OTAngle = Mathf.Atan(OTPosVert.y / OTPosVert.z);
+
+            //get correcting angle for FOV
+            float correctAngleDeg = ARucoWebcam.fieldOfView * OTAngle / ARAngle;
+
+            //add to list of measures
+            FOVCorrectionData.Add(correctAngleDeg);
+            FOVCorrectionData.Sort();
+
+            //get maximum angle correction
+            float maxAngle = FOVCorrectionData[FOVCorrectionData.Count - 1];
+
+            //apply correction to webcam camera
+            WebcamDisplay.fieldOfView = ARucoWebcam.fieldOfView + maxAngle;
+            WebcamInfosText.text = "FOV Calibration \n \n" +
+                "Angles tracked : " + FOVCorrectionData.Count + "\n" +
+                "Corrected vertical FOV : " + WebcamDisplay.fieldOfView + "� \n" +
+                "(R) Reset all \n" + 
+                "(W) Back to main scene \n" +
+                "(S) Switch to Axis calib.";
+
+        }
+    }
+
+    private void updateRotationFromValues()
     {
         if (calibrationData.Count > 0)
         {
@@ -146,13 +183,46 @@ public class WebcamCalibration : MonoBehaviour
                 meanDeviation += deltaAngle;
             });
 
-            WebcamInfosText.text = "Webcam Calibration \n \n" +
+            WebcamInfosText.text = "Axis Calibration \n \n" +
                 "Positions tracked : " + calibrationData.Count + "\n" +
                 "Deviation : " + System.Math.Round(meanDeviation / calibrationData.Count, 4) + "� \n" +
-                "(R) Reset";
+                "(R) Reset all \n" +
+                "(W) Back to main scene \n" +
+                "(S) Switch to FOV calib.";
+        } else
+        {
+            WebcamInfosText.text = "Axis Calibration \n \n" +
+                "Positions tracked : 0 \n" +
+                "Deviation : 0� \n" +
+                "(R) Reset all \n" +
+                "(W) Back to main scene \n" +
+                "(S) Switch to FOV calib.";
         }
     }
 
+    private void switchCalibrationFOVAxis()
+    {
+        doCalibrateFOV = !doCalibrateFOV;
+        //cancel current calibration
+        CancelInvoke();
+
+        if (doCalibrateFOV)
+        {
+            InvokeRepeating("calibrateFOV", 0.5f, 1f);
+            WebcamInfosText.text = "FOV Calibration \n \n" +
+                "Angles tracked : " + FOVCorrectionData.Count + "\n" +
+                "Vertical FOV : " + WebcamDisplay.fieldOfView + "� \n" +
+                "(R) Reset all \n" +
+                "(W) Back to main scene \n" +
+                "(S) Switch to Axis calib.";
+        } else
+        {
+            updateRotationFromValues();
+            InvokeRepeating("calibrateAxis", 0.5f, 1f);
+        }
+        
+    }
+
     // Update is called once per frame
     void Update()
     {
@@ -164,48 +234,60 @@ public class WebcamCalibration : MonoBehaviour
         {
             switchCalibrationMode();
         }
-
-        if (Input.GetKeyDown(KeyCode.R) && doCalibrate)
+        if (doCalibrate)
         {
-            calibrationData.Clear();
-            WebcamInfosText.text = "Webcam Calibration \n \n" +
-                "Positions tracked : 0 \n" +
-                "Deviation : 0� \n" +
-                "(R) Reset";
-        }
+            if (Input.GetKeyDown(KeyCode.R))
+            {
+                //remove axis data only in axis mode, but always reset FOV
+                if (!doCalibrateFOV)
+                {
+                    calibrationData.Clear();
+                }
+                FOVCorrectionData.Clear();
+                WebcamInfosText.text = (doCalibrateFOV ? "FOV Calibration" : "Axis Calibration") + "\n \n" +
+                    "Reset data done \n" +
+                    "Please calibrate new value";
+            }
 
-        if (Input.GetKeyDown(KeyCode.C) && !doCalibrate)
-        {
-            switchCalibrationAruco();
-        }
+            if (Input.GetKeyDown(KeyCode.S))
+            {
+                switchCalibrationFOVAxis();
+            }
 
-        if (Input.GetKey(KeyCode.DownArrow))
-        {
-            calibratedRotation = Quaternion.Euler(new Vector3(
-                calibratedRotation.eulerAngles.x + 0.01f,
-                calibratedRotation.eulerAngles.y,
-                calibratedRotation.eulerAngles.z));
-        }
-        else if (Input.GetKey(KeyCode.UpArrow))
-        {
-            calibratedRotation = Quaternion.Euler(new Vector3(
-                calibratedRotation.eulerAngles.x - 0.01f,
-                calibratedRotation.eulerAngles.y,
-                calibratedRotation.eulerAngles.z));
-        }
-        else if (Input.GetKey(KeyCode.LeftArrow))
-        {
-            calibratedRotation = Quaternion.Euler(new Vector3(
-                calibratedRotation.eulerAngles.x,
-                calibratedRotation.eulerAngles.y - 0.01f,
-                calibratedRotation.eulerAngles.z));
-        }
-        else if (Input.GetKey(KeyCode.RightArrow))
+            if (Input.GetKey(KeyCode.DownArrow))
+            {
+                calibratedRotation = Quaternion.Euler(new Vector3(
+                    calibratedRotation.eulerAngles.x + 0.01f,
+                    calibratedRotation.eulerAngles.y,
+                    calibratedRotation.eulerAngles.z));
+            }
+            else if (Input.GetKey(KeyCode.UpArrow))
+            {
+                calibratedRotation = Quaternion.Euler(new Vector3(
+                    calibratedRotation.eulerAngles.x - 0.01f,
+                    calibratedRotation.eulerAngles.y,
+                    calibratedRotation.eulerAngles.z));
+            }
+            else if (Input.GetKey(KeyCode.LeftArrow))
+            {
+                calibratedRotation = Quaternion.Euler(new Vector3(
+                    calibratedRotation.eulerAngles.x,
+                    calibratedRotation.eulerAngles.y - 0.01f,
+                    calibratedRotation.eulerAngles.z));
+            }
+            else if (Input.GetKey(KeyCode.RightArrow))
+            {
+                calibratedRotation = Quaternion.Euler(new Vector3(
+                    calibratedRotation.eulerAngles.x,
+                    calibratedRotation.eulerAngles.y + 0.01f,
+                    calibratedRotation.eulerAngles.z));
+            }
+        } else
         {
-            calibratedRotation = Quaternion.Euler(new Vector3(
-                calibratedRotation.eulerAngles.x,
-                calibratedRotation.eulerAngles.y + 0.01f,
-                calibratedRotation.eulerAngles.z));
+            if (Input.GetKeyDown(KeyCode.C))
+            {
+                switchCalibrationAruco();
+            }
         }
 
     }
diff --git a/Assets/WebcamVisionTexture.cs b/Assets/WebcamVisionTexture.cs
index b1c7a57593605477e5ec1ecae6eadb87fe0b5ea0..ddd7c906b7de0ca0f85557f9de69b18adc244320 100644
--- a/Assets/WebcamVisionTexture.cs
+++ b/Assets/WebcamVisionTexture.cs
@@ -3,6 +3,7 @@ using System.Collections.Generic;
 using UnityEngine;
 using OpenCvSharp;
 using System.Threading.Tasks;
+using ArucoUnity.Cameras.Undistortions;
 
 public class WebcamVisionTexture : MonoBehaviour
 {
@@ -11,6 +12,7 @@ public class WebcamVisionTexture : MonoBehaviour
     public GameObject ForegroundDisplay;
     public int maxImagesBG = 400;
     public int thresh;
+    public ArucoCameraUndistortion arucoUndistortion;
 
     private float aspectRatio;
 
@@ -21,6 +23,9 @@ public class WebcamVisionTexture : MonoBehaviour
     private Texture2D foregroundText;
     private int registeredImages = 0;
     private Point contourCenter;
+    private Mat map1;
+    private Mat map2;
+    private Mat maskBG;
 
     // Start is called before the first frame update
     void Start()
@@ -28,6 +33,8 @@ public class WebcamVisionTexture : MonoBehaviour
         startCam();
         registeredImages = maxImagesBG;
         currentImage = new Mat();
+        map1 = new Mat();
+        map2 = new Mat();
 
         //get image from webcam
         capture.Read(currentImage);
@@ -48,6 +55,13 @@ public class WebcamVisionTexture : MonoBehaviour
         registeredImages = 1;
         offlineBackground = new Mat();
         capture.Read(offlineBackground);
+        UndistortedMatrixGenerate();
+        //distort 1st bg image
+        Cv2.Remap(offlineBackground, offlineBackground, map1, map2);
+
+        //set border mask
+        maskBG = new Mat(offlineBackground.Size().Height, offlineBackground.Size().Width, MatType.CV_8UC1, new Scalar(255));
+        Cv2.Remap(maskBG, maskBG, map1, map2);
     }
 
     public void startCam()
@@ -73,6 +87,58 @@ public class WebcamVisionTexture : MonoBehaviour
         return aspectRatio;
     }
 
+    private void MatDisposer(Mat[] mats)
+    {
+        foreach(Mat mat in mats)
+        {
+            mat.Dispose();
+        }
+    }
+
+    private void UndistortedMatrixGenerate()
+    {
+        //get camera matrix to distort image
+        //convert camera matrix
+        ArucoUnity.Plugin.Cv.Mat arucoMat = arucoUndistortion.CameraParameters.CameraMatrices[WebcamID];
+        double[,] cameraMatrix = new double[arucoMat.Size.Height, arucoMat.Size.Width];
+        for (int i = 0; i < arucoMat.Size.Height; i++)
+        {
+            for (int j = 0; j < arucoMat.Size.Width; j++)
+            {
+                cameraMatrix[i, j] = arucoMat.AtDouble(i, j);
+            }
+        }
+
+        //convert dist coeffs
+        ArucoUnity.Plugin.Cv.Mat arucoDistCoefs = arucoUndistortion.CameraParameters.DistCoeffs[WebcamID];
+        double[] distCoefs = new double[arucoDistCoefs.Size.Width];
+        for (int i = 0; i < arucoDistCoefs.Size.Width; i++)
+        {
+            distCoefs[i] = arucoDistCoefs.AtDouble(0, i);
+        }
+
+        OpenCvSharp.Rect noROI = new OpenCvSharp.Rect();
+        double[,] RectifiedCameraMatrix = Cv2.GetOptimalNewCameraMatrix(cameraMatrix, distCoefs, offlineBackground.Size(), 1f, offlineBackground.Size(), out noROI, true);
+
+        /*offlineBackground.CopyTo(undistImg);
+        undistImg.Undistort(InputArray.Create(cameraMatrix), InputArray.Create(distCoefs), InputArray.Create(RectifiedCameraMatrix));
+        */
+
+        Mat r = new Mat();
+
+        Cv2.InitUndistortRectifyMap(
+            InputArray.Create(cameraMatrix),
+            InputArray.Create(distCoefs),
+            r,
+            InputArray.Create(RectifiedCameraMatrix),
+            offlineBackground.Size(),
+            MatType.CV_16SC2,
+            map1,
+            map2);
+
+        r.Dispose();
+    }
+
     private void UpdateForeground()
     {
         //Get the height and width of the Mat 
@@ -80,6 +146,8 @@ public class WebcamVisionTexture : MonoBehaviour
         int imgWidth = currentImage.Width;
 
         aspectRatio = imgWidth / (float) imgHeight;
+        Cv2.Remap(currentImage, currentImage, map1, map2);
+
         Mat diffImage = new Mat();
         Cv2.Absdiff(currentImage, offlineBackground, diffImage);
 
@@ -117,12 +185,6 @@ public class WebcamVisionTexture : MonoBehaviour
         }
         
 
-        //Cv2.AdaptiveThreshold(grayDiff, maskImg, 255, AdaptiveThresholdTypes.GaussianC, ThresholdTypes.Binary, 15, -5);
-        
-        
-
-        
-
         Mat maskedImg = new Mat();
         currentImage.CopyTo(maskedImg, maskImg);
 
@@ -179,11 +241,7 @@ public class WebcamVisionTexture : MonoBehaviour
             }
         });
 
-        kernel.Dispose();
-        diffImage.Dispose();
-        grayDiff.Dispose();
-        maskImg.Dispose();
-        maskedImg.Dispose();
+        MatDisposer(new Mat[] { kernel, diffImage, grayDiff, maskImg, maskedImg });
 
         foregroundText.SetPixels32(c);
         foregroundText.Apply();
@@ -193,6 +251,7 @@ public class WebcamVisionTexture : MonoBehaviour
 
     private void UpdateBackground()
     {
+        //currentImage is already warped from UpdateForeground()
         offlineBackground = (offlineBackground * registeredImages + currentImage) / ++registeredImages;
         if (registeredImages == maxImagesBG - 1)
             print("finished");
@@ -204,9 +263,13 @@ public class WebcamVisionTexture : MonoBehaviour
         aspectRatio = imgWidth / (float)imgHeight;
 
         Vec3b[] matData = new Vec3b[imgHeight * imgWidth];
+        byte[] maskData = new byte[imgHeight * imgWidth];
 
         //Get the byte array and store in matData
         offlineBackground.GetArray(out matData);
+        //get data from border mask
+        maskBG.GetArray(out maskData);
+
         //Create the Color array that will hold the pixels 
         Color32[] c = new Color32[imgHeight * imgWidth];
 
@@ -215,12 +278,13 @@ public class WebcamVisionTexture : MonoBehaviour
             for (var j = 0; j < imgWidth; j++)
             {
                 Vec3b vec = matData[j + (imgHeight - i - 1) * imgWidth];
+                byte maskVec = maskData[j + (imgHeight - i - 1) * imgWidth];
                 var color32 = new Color32
                 {
                     r = vec[2],
                     g = vec[1],
                     b = vec[0],
-                    a = 255
+                    a = maskVec
                 };
                 c[j + i * imgWidth] = color32;
             }
@@ -252,7 +316,6 @@ public class WebcamVisionTexture : MonoBehaviour
     private void OnDestroy()
     {
         stopCam();
-        currentImage.Dispose();
-        offlineBackground.Dispose();
+        MatDisposer(new Mat[] { currentImage, offlineBackground, map1, map2}); 
     }
 }