diff --git a/script/PlotCameraTrajectory.py b/script/PlotCameraTrajectory.py old mode 100644 new mode 100755 index 09ff0d2894..058c79ec3f --- 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 index 9653b5c8e0..fdaef06be0 100644 --- 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 index 1704418f10..b5e4e089d6 100644 --- a/tutorial/tracking/model-based/generic/tutorial-mb-generic-tracker-read.cpp +++ b/tutorial/tracking/model-based/generic/tutorial-mb-generic-tracker-read.cpp @@ -52,19 +52,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 index 9bcf723fb1..29f6a8f54b 100644 --- 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,8 +47,9 @@ 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; @@ -56,7 +57,7 @@ int main(int argc, char **argv) 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 {