Skip to content

Commit

Permalink
Merge pull request #1453 from s-trinh/feat_PlotCameraTrajectory_scrip…
Browse files Browse the repository at this point in the history
…t_recursive_model

Add possibility to load recursive CAO model with PlotCameraTrajectory.py script
  • Loading branch information
fspindle authored Aug 26, 2024
2 parents fa2e6bb + 84d0d80 commit 6a727de
Show file tree
Hide file tree
Showing 4 changed files with 181 additions and 35 deletions.
124 changes: 97 additions & 27 deletions script/PlotCameraTrajectory.py
100644 → 100755
Original file line number Diff line number Diff line change
@@ -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.
Expand All @@ -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)
Expand Down Expand Up @@ -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)}")
Expand Down Expand Up @@ -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 = []
Expand All @@ -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()
Expand All @@ -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}")
Expand Down Expand Up @@ -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)

Expand Down Expand Up @@ -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,
Expand All @@ -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.')
Expand All @@ -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
Expand Down Expand Up @@ -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}")
Expand Down Expand Up @@ -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)
Expand All @@ -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):
Expand All @@ -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)
Expand Down
78 changes: 76 additions & 2 deletions script/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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
Expand All @@ -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:
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down

0 comments on commit 6a727de

Please sign in to comment.