Merge pull request #1453 from s-trinh/feat_PlotCameraTrajectory_scrip…

…t_recursive_model Add possibility to load recursive CAO model with PlotCameraTrajectory.py script
lagadic · Aug 26, 2024 · 6a727de · 6a727de
2 parents fa2e6bb + 84d0d80
commit 6a727de
Show file tree

Hide file tree

Showing 4 changed files with 181 additions and 35 deletions.
diff --git a/script/PlotCameraTrajectory.py b/script/PlotCameraTrajectory.py
@@ -1,7 +1,7 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 # -*- coding: utf-8 -*-
 
-# Copyright 2022- ViSP contributor
+# Copyright 2022-2024 ViSP contributor
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
@@ -24,8 +24,9 @@
 import numpy as np
 import argparse
 import os
-
-debug_print = False
+import re
+from pathlib import Path # require >= Python 3.4
+import math
 
 def visp_thetau_to_rotation(thetau):
     theta = np.linalg.norm(thetau)
@@ -256,7 +257,7 @@ def draw_sphere(ax, tx, ty, tz, radius, color='b'):
     z = radius * np.cos(v) + tz
     ax.plot_wireframe(x, y, z, color=color)
 
-def draw_model(ax, model, color='b'):
+def draw_model(ax, model, debug_print, color='b'):
     # Lines
     if debug_print:
         print(f"model.lines_vec: {len(model.lines_vec)}")
@@ -520,7 +521,52 @@ def data_for_cylinder_along_z(center_x,center_y,radius,height_z):
 
     return x_grid,y_grid,z_grid
 
-def parse_cao_model(filename):
+def parse_load_recursive_model(input_str, debug_print):
+    filename = ""
+    t_vec = None
+    tu_vec = None
+    m = re.search('load\(\"(.+)\"', input_str)
+    if m:
+        filename = m.group(1)
+        print(f"filename={filename}")
+
+        pattern_t = '([-+]?\d*\.*\d+)'
+        m = re.search('t=\[{}; {}; {}\]'.format(pattern_t, pattern_t, pattern_t), input_str)
+        if m:
+            tx = float(m.group(1))
+            ty = float(m.group(2))
+            tz = float(m.group(3))
+            t_vec = [tx, ty, tz]
+            if debug_print:
+                print(f"tx={tx} ; ty={ty} ; tz={tz}")
+
+        pattern_unit = '(.rad|.deg)?'
+        pattern_tu_val = '([-+]?\d*\.*\d+)'
+        pattern_tu = '({}{})'.format(pattern_tu_val, pattern_unit)
+        m = re.search('tu=\[{}; {}; {}\]'.format(pattern_tu, pattern_tu, pattern_tu), input_str)
+        if m:
+            groups = m.groups()
+            if debug_print:
+                print(f"groups:\n{groups}")
+            tu_vec = []
+            for idx in range(3):
+                idx_tu_val = idx*3 + 1
+                idx_tu_unit = idx*3 + 2
+                tu_val = groups[idx_tu_val] if groups[idx_tu_unit] is None else \
+                    (groups[idx_tu_val] if "rad" in groups[idx_tu_unit] else math.radians(float(groups[idx_tu_val])))
+                tu_vec.append(float(tu_val))
+            if debug_print:
+                print(f"tu_vec={tu_vec}")
+
+    oTo = np.eye(4)
+    if tu_vec is not None:
+        oTo[:3,:3] = visp_thetau_to_rotation(tu_vec)
+    if t_vec is not None:
+        oTo[:3,3] = np.array(t_vec).T
+
+    return filename, oTo
+
+def parse_cao_model(filename, od_T_o, debug_print):
     with open(filename) as file:
         state = 0
         pts_vec = []
@@ -529,6 +575,7 @@ def parse_cao_model(filename):
         point_faces_vec = []
         cylinders_vec = []
         circles_vec = []
+        faces_vec = []
         max_iter = 10000
         for _ in range(max_iter):
             raw_line = file.readline()
@@ -543,20 +590,38 @@ def parse_cao_model(filename):
                     else:
                         raise ValueError("CAO model should have at the beginning the version number (either V0 or V1).")
                 elif state == 1:
-                    nb_pts = int(line)
-                    print(f"nb_pts: {nb_pts}")
-
-                    # Parse object points coordinates
-                    i = 0
-                    while i < nb_pts:
-                        raw_line = file.readline()
-                        if "" == raw_line:
-                            break
-                        line = remove_comment(raw_line.rstrip('\n').strip())
-                        if line:
-                            i += 1
-                            pts_vec.append([float(number) for number in line.split()])
-                    state = 2
+                    if line.startswith("load("):
+                        recursive_filename, oTo_local = parse_load_recursive_model(line, debug_print)
+                        print(f"oTo_local:\n{oTo_local}")
+
+                        path = Path(filename)
+                        recursive_filepath = path.parent / recursive_filename
+                        recursive_model = parse_cao_model(recursive_filepath, od_T_o @ oTo_local, debug_print)
+
+                        lines_vec.extend(recursive_model.lines_vec)
+                        point_faces_vec.extend(point_faces_vec)
+                        faces_vec.extend(recursive_model.faces_vec)
+                        cylinders_vec.extend(recursive_model.cylinders_vec)
+                        circles_vec.extend(recursive_model.spheres_vec)
+                    else:
+                        nb_pts = int(line)
+                        print(f"nb_pts: {nb_pts}")
+
+                        # Parse object points coordinates
+                        i = 0
+                        while i < nb_pts:
+                            raw_line = file.readline()
+                            if "" == raw_line:
+                                break
+                            line = remove_comment(raw_line.rstrip('\n').strip())
+                            if line:
+                                i += 1
+                                pts_vec_local_frame_vec = [float(number) for number in line.split()]
+                                pts_vec_local_frame_vec.append(1)
+                                pts_vec_local_frame = np.array(pts_vec_local_frame_vec)
+                                pts_vec_ref_frame = od_T_o @ pts_vec_local_frame
+                                pts_vec.append(pts_vec_ref_frame[:3])
+                        state = 2
                 elif state == 2:
                     nb_lines = int(line)
                     print(f"nb_lines: {nb_lines}")
@@ -677,7 +742,7 @@ def parse_cao_model(filename):
             print(f"cylinders_vec:\n{cylinders_vec}")
             print(f"circles_vec:\n{circles_vec}")
 
-        faces_vec = line_faces_vec
+        faces_vec.extend(line_faces_vec)
         faces_vec.extend(point_faces_vec)
         return Model(lines_vec, faces_vec, cylinders_vec, circles_vec)
 
@@ -720,7 +785,7 @@ def center_viewport(ax, x_lim, y_lim, z_lim, print_lim=False):
 
 def main():
     parser = argparse.ArgumentParser(description='Plot camera trajectory from poses and CAO model files.')
-    parser.add_argument('-p', type=str, nargs=1, required=True, help='Path to poses file.')
+    parser.add_argument('-p', type=str, nargs=1, required=True, help='Path to poses file (4n x 4 if homogeneous or lines of "tx ty tz tux tuy tuz").')
     parser.add_argument('--theta-u', action='store_true', default=False,
                         help='If true, camera poses are expressed using [tx ty tz tux tuy tuz] formalism, otherwise in homogeneous form.')
     parser.add_argument('--npz', action='store_true', default=False,
@@ -731,7 +796,6 @@ def main():
     parser.add_argument('--save-dir',  default='images', type=str, help='If --save flag is set, the folder where to save the plot images.')
     parser.add_argument('--save-pattern',  default='image_{:06d}.png', type=str, help='Image filename pattern when saving.')
     parser.add_argument('--save-dpi',  default=300, type=int, help='Image dpi when saving.')
-    parser.add_argument('--step', type=int, help='Step number between each camera poses when drawing.')
     parser.add_argument('--axes-label-size', type=int, default=20, help='Axes label size.')
     parser.add_argument('--xtick-label-size', type=int, default=14, help='X-tick label size.')
     parser.add_argument('--ytick-label-size', type=int, default=14, help='Y-tick label size.')
@@ -752,6 +816,7 @@ def main():
     parser.add_argument('--x-lim', type=float, nargs=2, default=[np.nan, np.nan], help='Manually set the x-limit for the viewport.')
     parser.add_argument('--y-lim', type=float, nargs=2, default=[np.nan, np.nan], help='Manually set the y-limit for the viewport.')
     parser.add_argument('--z-lim', type=float, nargs=2, default=[np.nan, np.nan], help='Manually set the z-limit for the viewport.')
+    parser.add_argument('--debug', action='store_true', default=False, help='Debug print.')
     args = parser.parse_args()
 
     axes_label_size = args.axes_label_size
@@ -795,10 +860,14 @@ def main():
     print(f"Colormap: {colormap}")
     camera_colors = get_colormap(camera_poses.shape[0]//4, colormap)
 
+    debug_print = args.debug
+    print(f"Debug print? {debug_print}")
+
     # Load model
     model_filename = args.m[0]
     print(f"Load object CAO model from: {model_filename}")
-    model = parse_cao_model(model_filename)
+    od_T_o = np.eye(4)
+    model = parse_cao_model(model_filename, od_T_o, debug_print)
     w_M_o = np.eye(4)
     w_M_o[:3,:3] = visp_thetau_to_rotation(np.array(args.wRo))
     print(f"w_M_o:\n{w_M_o}")
@@ -839,7 +908,7 @@ def main():
                 draw_camera(ax, camera_pose, cam_width, cam_height, cam_focal, cam_scale, camera_colors[i//4])
 
             draw_camera_path(ax, inverse_camera_poses[:cpt+8,:], camera_colors)
-            draw_model(ax, model)
+            draw_model(ax, model, debug_print)
             # Draw current camera pose
             draw_camera(ax, inverse_camera_poses[cpt:cpt+4,:], cam_width, cam_height, cam_focal, cam_scale, camera_colors[cpt//4])
             draw_model_frame(ax, w_M_o[:3,:3], frame_size)
@@ -862,7 +931,8 @@ def main():
             plt.close()
     else:
         fig = plt.figure()
-        ax = fig.gca(projection='3d')
+        #ax = fig.gca(projection='3d') # doesn't work on macOS Sonoma 14.6.1
+        ax = fig.add_subplot(projection='3d')
         ax.set_aspect("auto")
 
         for i in range(0, inverse_camera_poses.shape[0], 4*pose_step):
@@ -875,7 +945,7 @@ def main():
             draw_camera(ax, camera_pose, cam_width, cam_height, cam_focal, cam_scale, camera_colors[-1])
 
         draw_camera_path(ax, inverse_camera_poses, camera_colors)
-        draw_model(ax, model)
+        draw_model(ax, model, debug_print)
         draw_model_frame(ax, w_M_o[:3,:3], frame_size)
 
         center_viewport(ax, x_lim, y_lim, z_lim, print_lim=True)

diff --git a/script/README.md b/script/README.md
@@ -122,7 +122,9 @@ To test this script:
 
 This Python script allows displaying camera poses along with the object of interest.
 
-Camera poses can be saved in `4x4` homogeneous matrix form or in `1x6` pose vector form (`[tx, ty, tz, tux, tuy, tuz]`), C++ example code using the homogeneous matrix format:
+Camera poses can be saved in `4x4` homogeneous matrix form or in `1x6` pose vector form (`[tx, ty, tz, tux, tuy, tuz]`).
+
+C++ example code using the `4x4` homogeneous matrix format:
 
 ```cpp
 std::ofstream file_pose("poses.txt", std::ios_base::app);
@@ -136,6 +138,11 @@ if (file_pose.is_open()) {
 }
 ```
 
+Camera poses can also be read from npz file format:
+- pass the `--npz` flag to indicate the input pose file is stored in npz file format
+- the script will read data from `vec_poses` array name
+- array should contains `{tx, ty, tz, tu.x, tu.y, tu.z}` information (`nb_poses x 6`)
+
 Script example (use the help option `-h` to display the available parameters):
 
 ```console
@@ -146,9 +153,76 @@ Following video shows the camera poses outputed by the ViSP model-based tracker
 
 [![Cube+cylinder tracking using ViSP MBT](https://user-images.githubusercontent.com/8035162/180662930-605b2c42-bbb5-4bd6-9fe6-b2a1af3a04e7.png)](https://user-images.githubusercontent.com/8035162/180662750-02fccaff-74bd-411c-8258-06910edc6fde.mp4 "Cube+cylinder tracking using ViSP MBT")
 
+- Full tracking pipeline using teabox example:
+  1. Save teabox poses in npz format
+      ```console
+      % cd $VISP_WS/visp-build/tutorial/tracking/model-based/generic
+      % ./tutorial-mb-generic-tracker-full \
+            --video model/teabox/teabox.mp4 \
+            --model model/teabox/teabox.cao \
+            --tracker 2 \
+            --save-results teabox_tracking_results.npz
+      ```
+  2. Visualize camera trajectories
+      ```console
+      % cd $VISP_WS/visp/script
+      % python3 -m venv venv
+      % source venv/bin/activate
+      % python3 -m pip install matplotlib
+      % python3 PlotCameraTrajectory.py \
+            -p ../../visp-build/tutorial/tracking/model-based/generic/teabox_tracking_results.npz \
+            --npz \
+            -m ../../visp-build/tutorial/tracking/model-based/generic/model/teabox/teabox.cao
+      ```
+
+- Full tracking pipeline using cube example:
+  1. Save cube poses in npz format
+      ```console
+      % cd $VISP_WS/visp-build/tutorial/tracking/model-based/generic
+      % ./tutorial-mb-generic-tracker-full \
+            --video $VISP_WS/visp-images/mbt/cube/image%04d.png \
+            --model $VISP_WS/visp-images/mbt/cube.cao \
+            --tracker 2 \
+            --save-results cube_tracking_results.npz
+      ```
+  2. Visualize camera trajectories
+      ```console
+      % cd $VISP_WS/visp/script
+      % python3 -m venv venv
+      % source venv/bin/activate
+      % python3 -m pip install matplotlib
+      % python3 PlotCameraTrajectory.py \
+            -p ../../visp-build/tutorial/tracking/model-based/generic/cube_tracking_results.npz \
+            --npz \
+            -m $VISP_WS/visp-images/mbt/cube.cao
+      ```
+
+- Full tracking pipeline using cube & cylinder example:
+  1. Save cube & cylinder poses in npz format
+      ```console
+      % cd $VISP_WS/visp-build/tutorial/tracking/model-based/generic
+      % ./tutorial-mb-generic-tracker-full \
+            --video $VISP_WS/visp-images/mbt/cube/image%04d.png \
+            --model $VISP_WS/visp-images/mbt/cube_and_cylinder.cao \
+            --tracker 2 \
+            --save-results cube_and_cylinder_tracking_results.npz
+      ```
+  2. Visualize camera trajectories
+      ```console
+      % cd $VISP_WS/visp/script
+      % python3 -m venv venv
+      % source venv/bin/activate
+      % python3 -m pip install matplotlib
+      % python3 PlotCameraTrajectory.py \
+            -p ../../visp-build/tutorial/tracking/model-based/generic/cube_and_cylinder_tracking_results.npz \
+            --npz \
+            -m $VISP_WS/visp-images/mbt/cube_and_cylinder.cao
+      ```
+
 ## PlotRGBIrDepthData.py
 
-This Python script allows displaying RGB/Infrared/Depth/Pointcloud data saved using the example/device/framegrabber/saveRealSenseData.cpp sample.
+This Python script allows displaying RGB/Infrared/Depth/Pointcloud data saved using the
+`example/device/framegrabber/saveRealSenseData.cpp` sample.
 It contains sample code to learn how to manipulate NumPy NPZ format, such as multi-dimensional array and display them with Matplotlib.
 
 Simply run the following command with the correct folder path:

diff --git a/tutorial/tracking/model-based/generic/tutorial-mb-generic-tracker-read.cpp b/tutorial/tracking/model-based/generic/tutorial-mb-generic-tracker-read.cpp
@@ -38,19 +38,20 @@ int main(int argc, char *argv[])
     if (std::string(argv[i]) == "--cv-backend") {
       opencv_backend = true;
     }
-    else if ((std::string(argv[i]) == "--read" || std::string(argv[i]) == "-i") && i+1 < argc) {
+    else if ((std::string(argv[i]) == "--read" || std::string(argv[i]) == "-i") && (i+1 < argc)) {
       npz_filename = argv[i+1];
+      ++i;
     }
-    else if (std::string(argv[i]) == "--print-cMo" && i+1 < argc) {
+    else if (std::string(argv[i]) == "--print-cMo") {
       print_cMo = true;
     }
-    else if (std::string(argv[i]) == "--dump" && i+1 < argc) {
+    else if (std::string(argv[i]) == "--dump") {
       dump_infos = true;
     }
     else {
       std::cout << "Options:" << std::endl;
       std::cout << "  --cv-backend   use OpenCV if available for in-memory PNG decoding" << std::endl;
-      std::cout << "  --read / -i    input filename" << std::endl;
+      std::cout << "  --read / -i    input filename in npz format" << std::endl;
       std::cout << "  --print-cMo    print cMo" << std::endl;
       std::cout << "  --dump         print all the data name in the file" << std::endl;
       return EXIT_SUCCESS;

diff --git a/tutorial/tracking/model-based/generic/tutorial-mb-generic-tracker-save.cpp b/tutorial/tracking/model-based/generic/tutorial-mb-generic-tracker-save.cpp
@@ -47,16 +47,17 @@ int main(int argc, char **argv)
     if (std::string(argv[i]) == "--cv-backend") {
       opencv_backend = true;
     }
-    else if ((std::string(argv[i]) == "--save" || std::string(argv[i]) == "-o") && i+1 < argc) {
+    else if ((std::string(argv[i]) == "--save" || std::string(argv[i]) == "-o") && (i+1 < argc)) {
       npz_filename = argv[i+1];
+      ++i;
     }
     else if (std::string(argv[i]) == "--color" || std::string(argv[i]) == "-c") {
       color_mode = true;
     }
     else if (std::string(argv[i]) == "--alpha" || std::string(argv[i]) == "-a") {
       save_alpha = true;
     }
-    else if (std::string(argv[i]) == "--print-cMo" && i+1 < argc) {
+    else if (std::string(argv[i]) == "--print-cMo") {
       print_cMo = true;
     }
     else {