Adding AR environment demonstration.

6 files changed, 268 insertions, 7 deletions

diff --git a/src/argaze/utils/README.md b/src/argaze/utils/README.md
index 719218d..251cbf5 100644
--- a/src/argaze/utils/README.md
+++ b/src/argaze/utils/README.md
@@ -8,25 +8,40 @@ Collection of ready-to-use commands based on ArGaze toolkit.
 
 # ArUco Markers factory
 
-Export all markers from *DICT_APRILTAG_16h5* dictionary as 5 cm pictures with 300 dpi resolution into an *\_export/markers* folder:
+Export all markers from *DICT_APRILTAG_16h5* dictionary as 5 cm pictures with 300 dpi resolution into an *./src/argaze/utils/_export/markers* folder:
 
 ```
-python ./src/argaze/utils/aruco_markers_dictionary_export.py DICT_APRILTAG_16h5 -s 5 -r 300 -o _export/markers
+python ./src/argaze/utils/aruco_markers_dictionary_export.py DICT_APRILTAG_16h5 -s 5 -r 300 -o ./src/argaze/utils/_export/markers
 ```
 
-Export a 7 columns and 5 rows calibration board made of 5cm squares with 3cm markers from *DICT_APRILTAG_16h5* dictionary at 300 dpi into an *\_export* folder:
+Export a 7 columns and 5 rows calibration board made of 5cm squares with 3cm markers from *DICT_APRILTAG_16h5* dictionary at 300 dpi into an *./src/argaze/utils/_export* folder:
 
 ```
-python ./src/argaze/utils/aruco_calibration_board_export.py 7 5 5 3 DICT_APRILTAG_16h5 -r 300 -o _export
+python ./src/argaze/utils/aruco_calibration_board_export.py 7 5 5 3 DICT_APRILTAG_16h5 -r 300 -o ./src/argaze/utils/_export
 ```
 
 # Camera calibration
 
-Calibrate a camera device (-d DEVICE) using a 7 columns and 5 rows calibration board made of 5cm squares with 3cm markers from *DICT_APRILTAG_16h5* dictionary. Then, export its optical parameters into an *camera.json* file:
+Calibrate a camera device (-d DEVICE) using a 7 columns and 5 rows calibration board made of 5cm squares with 3cm markers from *DICT_APRILTAG_16h5* dictionary. Then, export its optical parameters into an *calibration.json* file in to *./src/argaze/utils/ar_environment* folder:
 
 ```
-python ./src/argaze/utils/camera_calibrate.py 7 5 5 3 DICT_APRILTAG_16h5 -d DEVICE -o _export
+python ./src/argaze/utils/camera_calibrate.py 7 5 5 3 DICT_APRILTAG_16h5 -d DEVICE -o ./src/argaze/utils/ar_environment
 ```
 
 .. note::
-   Use **A3_DICT_APRILTAG_16h5_3cm_35cmx25cm.pdf** file located in ./src/argaze/ArUcoMarkers/utils/ folder ready to be printed on A3 paper sheet.
\ No newline at end of file
+   Use **A3_DICT_APRILTAG_16h5_3cm_35cmx25cm.pdf** file located in *./src/argaze/ArUcoMarkers/utils/* folder ready to be printed on A3 paper sheet.
+
+# AR environment demonstration
+
+Load AR environment from **setup.json** file, detect ArUco markers into camera device (-d DEVICE) frames and estimate envirnoment pose.
+
+```
+python ./src/argaze/utils/demo_environment_run.py./src/argaze/utils/demo_environment/setup.json -dDEVICE
+```
+
+.. note::
+   This demonstration assumes that a **calibration.json** has been exported into *./src/argaze/utils/demo_environment/* folder.
+
+
+.. note::
+   Use **A3_demo.pdf** file located in *./src/argaze/utils/demo_environment/* folder ready to be printed on A3 paper sheet.
\ No newline at end of file
diff --git a/src/argaze/utils/demo_environment/A3_demo.pdf b/src/argaze/utils/demo_environment/A3_demo.pdf
new file mode 100644
index 0000000..cc51bc2
--- /dev/null
+++ b/src/argaze/utils/demo_environment/A3_demo.pdf
diff --git a/src/argaze/utils/demo_environment/aoi_scene.obj b/src/argaze/utils/demo_environment/aoi_scene.obj
new file mode 100644
index 0000000..e29d94c
--- /dev/null
+++ b/src/argaze/utils/demo_environment/aoi_scene.obj
@@ -0,0 +1,57 @@
+# Blender v3.0.1 OBJ File: 'ar_environment.blend'
+# www.blender.org
+o Screen
+v 0.000000 0.000000 0.000000
+v 25.000000 0.000000 0.000000
+v 0.000000 14.960000 0.000000
+v 25.000000 14.960000 0.000000
+s off
+f 1 2 4 3
+o RedSquare
+v 3.497026 8.309391 0.000000
+v 7.504756 8.309391 0.000000
+v 3.497026 12.314838 0.001030
+v 7.504756 12.314838 0.001030
+s off
+f 5 6 8 7
+o BlueTriangle
+v 10.500295 2.307687 0.000000
+v 14.503224 2.306344 0.000000
+v 12.502419 6.312207 0.001030
+s off
+f 9 10 11
+o GreenCircle
+v 19.495552 12.311101 0.000000
+v 19.105371 12.272672 0.000000
+v 18.730185 12.158860 0.000000
+v 18.384411 11.974040 0.000000
+v 18.081339 11.725314 0.000000
+v 17.832613 11.422241 0.000000
+v 17.647793 11.076468 0.000000
+v 17.533981 10.701282 0.000000
+v 17.495552 10.311101 0.000000
+v 17.533981 9.920920 0.000000
+v 17.647793 9.545734 0.000000
+v 17.832613 9.199961 0.000000
+v 18.081339 8.896888 0.000000
+v 18.384411 8.648162 0.000000
+v 18.730185 8.463342 0.000000
+v 19.105371 8.349530 0.000000
+v 19.495552 8.311101 0.000000
+v 19.885733 8.349530 0.000000
+v 20.260920 8.463342 0.000000
+v 20.606693 8.648162 0.000000
+v 20.909765 8.896887 0.000000
+v 21.158491 9.199960 0.000000
+v 21.343311 9.545733 0.000000
+v 21.457123 9.920920 0.000000
+v 21.495552 10.311101 0.000000
+v 21.457123 10.701282 0.000000
+v 21.343311 11.076468 0.000000
+v 21.158491 11.422241 0.000000
+v 20.909765 11.725314 0.000000
+v 20.606693 11.974040 0.000000
+v 20.260920 12.158860 0.000000
+v 19.885733 12.272672 0.000000
+s off
+f 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 12
diff --git a/src/argaze/utils/demo_environment/aruco_scene.obj b/src/argaze/utils/demo_environment/aruco_scene.obj
new file mode 100644
index 0000000..9ad43be
--- /dev/null
+++ b/src/argaze/utils/demo_environment/aruco_scene.obj
@@ -0,0 +1,34 @@
+# Blender v3.0.1 OBJ File: 'ar_environment.blend'
+# www.blender.org
+o DICT_APRILTAG_16h5#0_Marker
+v -5.000000 14.960000 0.000000
+v 0.000000 14.960000 0.000000
+v -5.000000 19.959999 0.000000
+v 0.000000 19.959999 0.000000
+vn 0.0000 0.0000 1.0000
+s off
+f 1//1 2//1 4//1 3//1
+o DICT_APRILTAG_16h5#1_Marker
+v 25.000000 14.960000 0.000000
+v 30.000000 14.960000 0.000000
+v 25.000000 19.959999 0.000000
+v 30.000000 19.959999 0.000000
+vn 0.0000 0.0000 1.0000
+s off
+f 5//2 6//2 8//2 7//2
+o DICT_APRILTAG_16h5#2_Marker
+v -5.000000 -5.000000 0.000000
+v 0.000000 -5.000000 0.000000
+v -5.000000 0.000000 0.000000
+v 0.000000 0.000000 0.000000
+vn 0.0000 0.0000 1.0000
+s off
+f 9//3 10//3 12//3 11//3
+o DICT_APRILTAG_16h5#3_Marker
+v 25.000000 -5.000000 0.000000
+v 30.000000 -5.000000 0.000000
+v 25.000000 0.000000 0.000000
+v 30.000000 0.000000 0.000000
+vn 0.0000 0.0000 1.0000
+s off
+f 13//4 14//4 16//4 15//4
diff --git a/src/argaze/utils/demo_environment/setup.json b/src/argaze/utils/demo_environment/setup.json
new file mode 100644
index 0000000..7810f5f
--- /dev/null
+++ b/src/argaze/utils/demo_environment/setup.json
@@ -0,0 +1,62 @@
+{
+    "name": "AR Environment Demo",
+    "aruco_detector": {
+        "dictionary": "DICT_APRILTAG_16h5",
+        "marker_size": 5,
+        "camera": "calibration.json",
+        "parameters": {
+            "cornerRefinementMethod": 1,
+            "aprilTagQuadSigma": 2,
+            "aprilTagDeglitch": 1
+        }
+    },
+    "AR Scene Demo" : {
+        "aruco_scene": "aruco_scene.obj",
+        "aoi_scene": "aoi_scene.obj",
+        "aruco_axis": {
+            "lower_left_corner": {
+                "origin_marker": 2,
+                "horizontal_axis_marker": 3,
+                "vertical_axis_marker": 0
+            },
+            "lower_right_corner": {
+                "origin_marker": 3,
+                "horizontal_axis_marker": 2,
+                "vertical_axis_marker": 1
+            },
+            "upper_left_corner": {
+                "origin_marker": 0,
+                "horizontal_axis_marker": 1,
+                "vertical_axis_marker": 2
+            },
+            "upper_right_corner": {
+                "origin_marker": 1,
+                "horizontal_axis_marker": 0,
+                "vertical_axis_marker": 3
+            }
+        },
+        "aruco_aoi": {
+            "Screen_Plan": {
+                "upper_left_corner": {
+                    "marker_identifier": 0,
+                    "marker_corner_index": 2
+                },
+                "upper_right_corner": {
+                    "marker_identifier": 1,
+                    "marker_corner_index": 3
+                },
+                "lower_left_corner": {
+                    "marker_identifier": 2,
+                    "marker_corner_index": 1
+                },
+                "lower_right_corner": {
+                    "marker_identifier": 3,
+                    "marker_corner_index": 0
+                },
+                "inner_aoi": "all"
+            }
+        },
+        "angle_tolerance": 15.0,
+        "distance_tolerance": 2.54
+    }
+}
\ No newline at end of file
diff --git a/src/argaze/utils/demo_environment_run.py b/src/argaze/utils/demo_environment_run.py
new file mode 100644
index 0000000..b2cd1e7
--- /dev/null
+++ b/src/argaze/utils/demo_environment_run.py
@@ -0,0 +1,93 @@
+#!/usr/bin/env python
+
+import argparse
+import os
+import time
+
+from argaze import ArFeatures
+
+import cv2
+
+def main():
+    """
+    Load AR environment from .json file, detect ArUco markers into camera device frames and estimate environment pose.
+    """
+
+    # Manage arguments
+    parser = argparse.ArgumentParser(description=main.__doc__.split('-')[0])
+    parser.add_argument('environment', metavar='ENVIRONMENT', type=str, help='ar environment filepath')
+
+    parser.add_argument('-d', '--device', metavar='DEVICE', type=int, default=0, help='video capture device id')
+    args = parser.parse_args()
+
+    # Load AR enviroment
+    demo_environment = ArFeatures.ArEnvironment.from_json(args.environment)
+
+    print('ArEnvironment:\n', demo_environment)
+
+    # Access to main AR scene
+    demo_scene = demo_environment.scenes["AR Scene Demo"]
+
+    # Enable camera video capture
+    video_capture = cv2.VideoCapture(args.device)
+
+    # Capture loop
+    try:
+
+        # Capture frames with a full displayed board inside
+        while video_capture.isOpened():
+
+            success, video_frame = video_capture.read()
+
+            if success:
+
+                # Detect markers
+                demo_environment.aruco_detector.detect_markers(video_frame)
+
+                # Draw detected markers
+                demo_environment.aruco_detector.draw_detected_markers(video_frame)
+
+                 # Process detected markers for Matsuri scene
+                try:
+
+                    try:
+                        # Try to build AOI scene from detected ArUco marker corners
+                        scene_projection = demo_scene.build_aruco_aoi_scene(demo_environment.aruco_detector.detected_markers)
+
+                    except:
+
+                        # Estimate scene markers poses
+                        demo_environment.aruco_detector.estimate_markers_pose(demo_scene.aruco_scene.identifiers)
+
+                        # Estimate scene pose from detected scene markers
+                        tvec, rmat, _ = demo_scene.estimate_pose(demo_environment.aruco_detector.detected_markers)
+
+                        # Project AOI scene into frame according estimated pose
+                        scene_projection = demo_scene.project(tvec, rmat)
+
+                    # Draw AOI
+                    scene_projection.draw(video_frame, (0, 0), color=(0, 255, 255))
+
+                # Catch exceptions raised by estimate_pose and project methods
+                except (ArFeatures.PoseEstimationFailed, ArFeatures.SceneProjectionFailed) as e:
+
+                    cv2.rectangle(video_frame, (0, 50), (700, 100), (127, 127, 127), -1)
+                    cv2.putText(video_frame, f'Error: {e}', (20, 80), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv2.LINE_AA)
+            
+                # Draw frame
+                cv2.imshow(demo_environment.name, video_frame)
+
+            # Stop calibration by pressing 'Esc' key
+            if cv2.waitKey(1) == 27:
+                break
+
+    # Stop calibration on 'ctrl+C' interruption
+    except KeyboardInterrupt:
+        pass
+
+    # Stop frame display
+    cv2.destroyAllWindows()
+
+if __name__ == '__main__':
+
+    main()