Skip to content

Commit 3f326b5

Browse files
chutsuchutsu
committed
Update
1 parent 1fcb420 commit 3f326b5

File tree

5 files changed

+300
-22
lines changed

5 files changed

+300
-22
lines changed

‎scripts/calib_gimbal.py

+2-2
Original file line numberDiff line numberDiff line change
@@ -256,5 +256,5 @@ def create_gimbal_calib_config(rs0_results, rs1_results, save_path):
256256
# os.system(f"./build/calib_vi {calib_camimu_path} {calib_camimu_rs1}")
257257

258258
# Create gimbal calibration file
259-
create_gimbal_calib_config(rs0_camimu_results, rs1_camimu_results,
260-
calib_gimbal)
259+
# create_gimbal_calib_config(rs0_camimu_results, rs1_camimu_results,
260+
# calib_gimbal)

‎scripts/plot_gimbal_frames2.py

+208
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,208 @@
1+
#!/usr/bin/env python3
2+
""" Plot gimbal frames """
3+
import os
4+
import sys
5+
import yaml
6+
import math
7+
import glob
8+
9+
from proto import quat2rot
10+
from proto import euler321
11+
from proto import rot2quat
12+
from proto import rotz
13+
from proto import tf
14+
from proto import tf_trans
15+
from proto import tf_quat
16+
from proto import AprilGrid
17+
from proto import plot_tf
18+
from proto import plot_set_axes_equal
19+
20+
import cv2
21+
import numpy as np
22+
import matplotlib.pylab as plt
23+
from matplotlib import animation
24+
25+
26+
def form_transform(pose):
27+
""" Form 4x4 Transformation Matrix from pose vector """
28+
x, y, z = pose[0:3]
29+
qw, qx, qy, qz = pose[3:7]
30+
r = np.array([x, y, z])
31+
C = quat2rot(np.array([qw, qx, qy, qz]))
32+
return tf(C, r)
33+
34+
35+
def form_pose(T):
36+
""" Form pose vector from 4x4 Transformation matrix """
37+
r = tf_trans(T)
38+
q = tf_quat(T)
39+
return np.array([*r, *q])
40+
41+
42+
def gimbal_joint_transform(theta):
43+
""" Form 4x4 Transformation Matrix from joint angle """
44+
r = np.array([0.0, 0.0, 0.0])
45+
C = rotz(theta)
46+
return tf(C, r)
47+
48+
49+
def load_calib_config(data_path):
50+
calib_config_file = open(f"{data_path}/calib_gimbal-results.yaml", "r")
51+
calib_config = yaml.safe_load(calib_config_file)
52+
return calib_config
53+
54+
def load_joint_data(data_path):
55+
joints_file = open(f"{data_path}/joint_angles.dat", "r")
56+
joints_lines = joints_file.read().split("\n")
57+
58+
_, num_views = joints_lines[0].split(":")
59+
_, num_joints = joints_lines[1].split(":")
60+
61+
timestamps = []
62+
joint0_data = {}
63+
joint1_data = {}
64+
joint2_data = {}
65+
for i in range(int(num_views)):
66+
ts, joint0, joint1, joint2 = joints_lines[4 + i].split(",")
67+
ts = int(ts)
68+
timestamps.append(ts)
69+
joint0_data[ts] = float(joint0)
70+
joint1_data[ts] = float(joint1)
71+
joint2_data[ts] = float(joint2)
72+
73+
return {"timestamps": timestamps, "joint0": joint0_data, "joint1": joint1_data, "joint2": joint2_data}
74+
75+
76+
def load_camera_data(data_path):
77+
cam0_images = {}
78+
for cam0_file in glob.glob(f"{data_path}/cam0/*.png"):
79+
fname = os.path.basename(cam0_file).replace(".png", "")
80+
ts = int(fname)
81+
img = cv2.imread(cam0_file)
82+
cam0_images[ts] = img
83+
84+
cam1_images = {}
85+
for cam1_file in glob.glob(f"{data_path}/cam1/*.png"):
86+
fname = os.path.basename(cam1_file).replace(".png", "")
87+
ts = int(fname)
88+
img = cv2.imread(cam1_file)
89+
cam1_images[ts] = img
90+
91+
cam2_images = {}
92+
for cam2_file in glob.glob(f"{data_path}/cam2/*.png"):
93+
fname = os.path.basename(cam2_file).replace(".png", "")
94+
ts = int(fname)
95+
img = cv2.imread(cam2_file)
96+
cam2_images[ts] = img
97+
98+
cam3_images = {}
99+
for cam3_file in glob.glob(f"{data_path}/cam3/*.png"):
100+
fname = os.path.basename(cam3_file).replace(".png", "")
101+
ts = int(fname)
102+
img = cv2.imread(cam3_file)
103+
cam3_images[ts] = img
104+
105+
return {
106+
0: cam0_images,
107+
1: cam1_images,
108+
2: cam2_images,
109+
3: cam3_images
110+
}
111+
112+
113+
def remove_tf(tf):
114+
tf[0].remove()
115+
tf[1].remove()
116+
tf[2].remove()
117+
tf[3].remove()
118+
119+
120+
if __name__ == "__main__":
121+
# Load data
122+
data_path = "/data/gimbal_experiments/exp-240220/calib-240212/calib_gimbal"
123+
calib_config = load_calib_config(data_path)
124+
joint_data = load_joint_data(data_path)
125+
camera_data = load_camera_data(data_path)
126+
127+
# Transforms
128+
T_C0M0 = form_transform(calib_config["gimbal_ext"])
129+
T_L0M1 = form_transform(calib_config["link0_ext"])
130+
T_L1M2 = form_transform(calib_config["link1_ext"])
131+
T_L2C2 = form_transform(calib_config["end_ext"])
132+
T_C0C0 = form_transform(calib_config["cam0_ext"])
133+
T_C0C1 = form_transform(calib_config["cam1_ext"])
134+
T_C2C2 = form_transform(calib_config["cam2_ext"])
135+
T_C2C3 = form_transform(calib_config["cam3_ext"])
136+
137+
# yapf:disable
138+
T_WC0 = np.array([
139+
[0.000000, 0.000000, 1.000000, 0.000000],
140+
[-1.000000, 0.000000, 0.000000, 0.000000],
141+
[0.000000, -1.000000, 0.000000, 0.000000],
142+
[0.000000, 0.000000, 0.000000, 1.000000]
143+
])
144+
T_WC1 = T_WC0 @ T_C0C1
145+
# yapf:enable
146+
147+
print(f"T_C0M0:\n{T_C0M0}\n")
148+
print(f"T_L0M1:\n{T_L0M1}\n")
149+
print(f"T_L1M2:\n{T_L1M2}\n")
150+
print(f"T_L2C2:\n{T_L2C2}\n")
151+
print(f"T_C0C1:\n{T_C0C1}\n")
152+
print(f"T_C2C3:\n{T_C2C3}\n")
153+
154+
# Setup plot
155+
fs = 0.01
156+
fig = plt.figure()
157+
ax0 = fig.add_subplot(1, 1, 1, projection="3d")
158+
ax0.set_xlabel("x [m]")
159+
ax0.set_ylabel("y [m]")
160+
ax0.set_zlabel("z [m]")
161+
162+
plt.draw()
163+
plt.pause(1)
164+
plt.ion()
165+
plt.show()
166+
167+
# Loop over timestamps
168+
for ts in joint_data["timestamps"]:
169+
joint0 = joint_data["joint0"][ts]
170+
joint1 = joint_data["joint1"][ts]
171+
joint2 = joint_data["joint2"][ts]
172+
173+
T_M0L0 = gimbal_joint_transform(joint0)
174+
T_M1L1 = gimbal_joint_transform(joint1)
175+
T_M2L2 = gimbal_joint_transform(joint2)
176+
177+
T_WL0 = T_WC0 @ T_C0M0 @ T_M0L0
178+
T_WL1 = T_WL0 @ T_L0M1 @ T_M1L1
179+
T_WL2 = T_WL1 @ T_L1M2 @ T_M2L2
180+
T_WC2 = T_WL2 @ T_L2C2
181+
T_WC3 = T_WL2 @ T_L2C2 @ T_C2C3
182+
183+
# Plot frames
184+
plot_T_WL0 = plot_tf(ax0, T_WL0, name="joint0", size=fs)
185+
plot_T_WL1 = plot_tf(ax0, T_WL1, name="joint1", size=fs)
186+
plot_T_WL2 = plot_tf(ax0, T_WL2, name="joint2", size=fs)
187+
plot_T_WC0 = plot_tf(ax0, T_WC0, name="cam0", size=fs)
188+
plot_T_WC1 = plot_tf(ax0, T_WC1, name="cam1", size=fs)
189+
plot_T_WC2 = plot_tf(ax0, T_WC2, name="cam2", size=fs)
190+
plot_T_WC3 = plot_tf(ax0, T_WC3, name="cam3", size=fs)
191+
plot_set_axes_equal(ax0)
192+
193+
# Draw
194+
plt.draw()
195+
plt.pause(1)
196+
197+
# Plot cam0 image
198+
cv2.imshow("Viz", camera_data[2][ts])
199+
cv2.waitKey(1)
200+
201+
# Remove frames
202+
remove_tf(plot_T_WL0)
203+
remove_tf(plot_T_WL1)
204+
remove_tf(plot_T_WL2)
205+
remove_tf(plot_T_WC0)
206+
remove_tf(plot_T_WC1)
207+
remove_tf(plot_T_WC2)
208+
remove_tf(plot_T_WC3)

‎scripts/run.sh

+14-2
Original file line numberDiff line numberDiff line change
@@ -17,7 +17,7 @@ set -e
1717
# python3 scripts/plot_xyz.py /tmp/vel.csv "#ts" vx vy vz && exit
1818
# python3 scripts/plot_xyz.py /tmp/acc.csv "#ts" ax ay az && exit
1919
# python3 scripts/plot_xyz.py /tmp/gyr.csv "#ts" wx wy wz && exit
20-
python3 scripts/calib_gimbal.py
20+
# python3 scripts/calib_gimbal.py
2121
# python3 scripts/sandbox.py && exit
2222
# python3 scripts/lissajous.py && exit
2323
# python3 scripts/lissajous.py > /tmp/out.txt && exit
@@ -150,13 +150,24 @@ python3 scripts/calib_gimbal.py
150150
# rm -rf /tmp/calib_data/calib_imu
151151
# rm -rf /tmp/calib_data/calib_camera
152152

153+
# tmux send-keys -t dev -R C-l C-m
154+
# tmux send-keys -t dev -R "\
155+
# cd ~/projects/yac/build \
156+
# && cmake --build . --target calib_gimbal2 .. \
157+
# && ./calib_gimbal2 /data/gimbal_experiments/calib/calib_gimbal
158+
# " c-m
159+
# exit
160+
153161
tmux send-keys -t dev -R C-l C-m
154162
tmux send-keys -t dev -R "\
155163
cd ~/projects/yac/build \
156164
&& cmake --build . --target calib_gimbal2 .. \
157-
&& ./calib_gimbal2
165+
&& ./calib_gimbal2 /data/gimbal_experiments/exp-240703/calib/calib_gimbal
158166
" c-m
159167
exit
168+
# && ./calib_gimbal2 /data/gimbal_experiments/exp-240303/calib/calib_gimbal/
169+
# && ./calib_gimbal2 /data/gimbal_experiments/exp-240220/calib-240212/calib_gimbal/
170+
# && ./calib_gimbal2 /data/gimbal_experiments/exp-240220/calib-240225/calib_gimbal/
160171

161172
# tmux send-keys -t dev -R C-l C-m
162173
# tmux send-keys -t dev -R "\
@@ -168,6 +179,7 @@ exit
168179
# exit
169180

170181
# python3 scripts/plot_gimbal_frames.py
182+
# python3 scripts/plot_gimbal_frames2.py
171183

172184
# make
173185
# make lib_debug

‎yac/CMakeLists.txt

+4-4
Original file line numberDiff line numberDiff line change
@@ -57,10 +57,10 @@ SET(DEPS
5757
apriltag # Ed Olsen
5858
apriltags # Michael Kaess
5959
${OpenCV_LIBS}
60-
${CHOLMOD_LIBRARY}
61-
${SUITESPARSEQR_LIBRARY}
62-
${SUITESPARSE_CONFIG_LIBRARY}
63-
${TBB_LIBRARY}
60+
${CHOLMOD_LIBRARY}
61+
${SUITESPARSEQR_LIBRARY}
62+
${SUITESPARSE_CONFIG_LIBRARY}
63+
${TBB_LIBRARY}
6464
)
6565

6666
# LIBRARY

0 commit comments

Comments
 (0)