def __init__(self, intrinsics_file):
self.intrinsics = CameraIntrinsics(intrinsics_file)
# Chessboard
self.chessboard = Chessboard(t_G=np.array([0.3, 0.1, 0.1]),
nb_rows=11,
nb_cols=11,
square_size=0.02)
self.plot_chessboard = PlotChessboard(chessboard=self.chessboard)
# Gimbal
self.gimbal = GimbalModel()
self.gimbal.set_attitude([0.0, 0.0, 0.0])
self.plot_gimbal = PlotGimbal(gimbal=self.gimbal)
# Cameras
self.static_camera = self.setup_static_camera()
self.gimbal_camera = self.setup_gimbal_camera()
def __init__(self, **kwargs):
self.origin = np.array([0.0, 0.0, 0.0])
self.gimbal = kwargs.get("gimbal", GimbalModel())
self.link0 = None
self.link1 = None
self.link2 = None
self.link3 = None
self.show_static_frame = kwargs.get("show_static_frame", True)
self.show_base_frame = kwargs.get("show_base_frame", True)
self.show_end_frame = kwargs.get("show_end_frame", True)
self.show_dynamic_frame = kwargs.get("show_dynamic_frame", True)
def __init__(self, **kwargs):
self.gimbal_model = kwargs.get("gimbal_model", GimbalModel())
if kwargs.get("sim_mode", False):
self.ec_data = kwargs["ec_data"]
self.joint_data = kwargs["joint_data"]
else:
self.loader = GECDataLoader(**kwargs)
if kwargs.get("preprocessed", False) is False:
# Calibration and joint data
self.ec_data, self.joint_data = self.loader.load()
# Camera intrinsics matrix
self.K_s = self.ec_data[0].intrinsics.K_new
self.K_d = self.ec_data[1].intrinsics.K_new
# Number of measurement set
self.K = len(self.ec_data[0].corners2d_ud)
# Number of links - 2 for a 2-axis gimbal
self.L = 2
# Setup measurement sets
self.Z = []
for i in range(self.K):
# Corners 3d observed in both the static and dynamic cam
P_s = self.ec_data[0].corners3d_ud[i]
P_d = self.ec_data[1].corners3d_ud[i]
# Corners 2d observed in both the static and dynamic cam
Q_s = self.ec_data[0].corners2d_ud[i]
Q_d = self.ec_data[1].corners2d_ud[i]
Z_i = [P_s, P_d, Q_s, Q_d]
self.Z.append(Z_i)
else:
# Measurement set and joint data
self.Z, self.K_s, self.K_d, self.joint_data = self.loader.load()
# Number of measurement set
self.K = len(self.Z)
# Number of links - 2 for a 2-axis gimbal
self.L = 2
def __init__(self, **kwargs):
self.gimbal_model = kwargs.get("gimbal_model", GimbalModel())
if kwargs.get("sim_mode", False):
# Load sim data
self.Z = kwargs["Z"]
self.K_s = kwargs["K_s"]
self.K_d = kwargs["K_d"]
self.D_s = kwargs["D_s"]
self.D_d = kwargs["D_d"]
self.joint_data = kwargs["joint_data"]
self.K = len(self.Z)
else:
# Load data
self.loader = DataLoader(**kwargs)
# -- Measurement set and joint data
data = self.loader.load()
self.Z, self.K_s, self.K_d, self.D_s, self.D_d, self.joint_data = data
# -- Number of measurement set
self.K = len(self.Z)
def test_optimize_preprocessed(self):
gimbal_model = GimbalModel(
tau_s=np.array([0.045, 0.075, -0.085, 0.0, 0.0, pi / 2.0]),
tau_d=np.array([0.0, 0.015, 0.0, 0.0, 0.0, -pi / 2.0]),
w1=np.array([0.0, 0.0, 0.075]),
w2=np.array([0.0, 0.0, 0.0])
)
# gimbal_model.set_attitude([deg2rad(0), deg2rad(0)])
# plot_gimbal = PlotGimbal(gimbal=gimbal_model)
# plot_gimbal.plot()
# plt.show()
data_path = "/home/chutsu/Dropbox/calib_data"
calib = GimbalCalibrator(
preprocessed=True,
gimbal_model=gimbal_model,
data_path=data_path,
data_dirs=["cam0", "cam1"],
intrinsic_files=["static_camera.yaml", "gimbal_camera.yaml"],
joint_file="joint.csv"
)
calib.optimize()
class GimbalDataGenerator:
def __init__(self, intrinsics_file):
self.intrinsics = CameraIntrinsics(intrinsics_file)
# Chessboard
self.chessboard = Chessboard(t_G=np.array([0.3, 0.1, 0.1]),
nb_rows=11,
nb_cols=11,
square_size=0.02)
self.plot_chessboard = PlotChessboard(chessboard=self.chessboard)
# Gimbal
self.gimbal = GimbalModel()
self.gimbal.set_attitude([0.0, 0.0, 0.0])
self.plot_gimbal = PlotGimbal(gimbal=self.gimbal)
# Cameras
self.static_camera = self.setup_static_camera()
self.gimbal_camera = self.setup_gimbal_camera()
def setup_static_camera(self):
image_width = 640
image_height = 480
fov = 120
fx, fy = focal_length(image_width, image_height, fov)
cx, cy = (image_width / 2.0, image_height / 2.0)
K = camera_intrinsics(fx, fy, cx, cy)
cam_model = PinholeCameraModel(image_width, image_height, K)
return cam_model
def setup_gimbal_camera(self):
image_width = 640
image_height = 480
fov = 120
fx, fy = focal_length(image_width, image_height, fov)
cx, cy = (image_width / 2.0, image_height / 2.0)
K = camera_intrinsics(fx, fy, cx, cy)
cam_model = PinholeCameraModel(image_width, image_height, K)
return cam_model
def calc_static_camera_view(self):
# Transforming chessboard grid points in global to camera frame
R = np.eye(3)
t = np.zeros(3)
R_CG = euler2rot([-pi / 2.0, 0.0, -pi / 2.0], 123)
X = dot(R_CG, self.chessboard.grid_points3d.T)
x = self.static_camera.project(X, R, t).T[:, 0:2]
return x
def calc_gimbal_camera_view(self):
# Create transform from global to static camera frame
t_g_sg = np.array([0.0, 0.0, 0.0])
R_sg = euler2rot([-pi / 2.0, 0.0, -pi / 2.0], 321)
T_gs = np.array([[R_sg[0, 0], R_sg[0, 1], R_sg[0, 2], t_g_sg[0]],
[R_sg[1, 0], R_sg[1, 1], R_sg[1, 2], t_g_sg[1]],
[R_sg[2, 0], R_sg[2, 1], R_sg[2, 2], t_g_sg[2]],
[0.0, 0.0, 0.0, 1.0]])
# Calculate transform from global to dynamic camera frame
links = self.gimbal.calc_transforms()
T_sd = links[-1]
T_gd = dot(T_gs, T_sd)
# Project chessboard grid points in global to dynamic camera frame
# -- Convert 3D points to homogeneous coordinates
X = self.chessboard.grid_points3d.T
X = np.block([[X], [np.ones(X.shape[1])]])
# -- Project to dynamica camera image frame
X = dot(np.linalg.inv(T_gd), X)[0:3, :]
x = dot(self.gimbal_camera.K, X)
# -- Normalize points
x[0, :] = x[0, :] / x[2, :]
x[1, :] = x[1, :] / x[2, :]
x = x[0:2, :].T
return x, X.T
def plot_static_camera_view(self, ax):
x = self.calc_static_camera_view()
ax.scatter(x[:, 0], x[:, 1], marker="o", color="red")
def plot_gimbal_camera_view(self, ax):
x, X = self.calc_gimbal_camera_view()
ax.scatter(x[:, 0], x[:, 1], marker="o", color="red")
def plot_camera_views(self):
# Plot static camera view
ax = plt.subplot(211)
ax.axis('square')
self.plot_static_camera_view(ax)
ax.set_title("Static Camera View", y=1.08)
ax.set_xlim((0, self.static_camera.image_width))
ax.set_ylim((0, self.static_camera.image_height))
ax.invert_yaxis()
ax.xaxis.tick_top()
# Plot gimbal camera view
ax = plt.subplot(212)
ax.axis('square')
self.plot_gimbal_camera_view(ax)
ax.set_title("Gimbal Camera View", y=1.08)
ax.set_xlim((0, self.gimbal_camera.image_width))
ax.set_ylim((0, self.gimbal_camera.image_height))
ax.invert_yaxis()
ax.xaxis.tick_top()
# Overall plot settings
plt.tight_layout()
def plot(self):
# Plot camera views
self.plot_camera_views()
# Plot gimbal and chessboard
fig = plt.figure()
ax = fig.gca(projection='3d')
self.plot_gimbal.plot(ax)
self.plot_chessboard.plot(ax)
axis_equal_3dplot(ax)
ax.set_xlabel("x")
ax.set_ylabel("y")
ax.set_zlabel("z")
plt.show()
def calc_roll_pitch_combo(self, nb_images):
nb_combo = int(sqrt(nb_images))
roll_lim = [radians(-10), radians(10)]
pitch_lim = [radians(-10), radians(10)]
roll_vals = np.linspace(roll_lim[0], roll_lim[1], num=nb_combo)
pitch_vals = np.linspace(pitch_lim[0], pitch_lim[1], num=nb_combo)
return roll_vals, pitch_vals
def generate(self):
# Setup
nb_images = 100
R_CG = euler2rot([-pi / 2.0, 0.0, -pi / 2.0], 123)
# Generate static camera data
self.intrinsics.K_new = self.intrinsics.K()
static_cam_data = ECData(None, self.intrinsics, self.chessboard)
x = self.calc_static_camera_view()
X = dot(R_CG, self.chessboard.grid_points3d.T).T
for i in range(nb_images):
static_cam_data.corners2d_ud.append(x)
static_cam_data.corners3d_ud.append(X)
static_cam_data.corners2d_ud = np.array(static_cam_data.corners2d_ud)
static_cam_data.corners3d_ud = np.array(static_cam_data.corners3d_ud)
# Generate gimbal data
roll_vals, pitch_vals = self.calc_roll_pitch_combo(nb_images)
gimbal_cam_data = ECData(None, self.intrinsics, self.chessboard)
joint_data = []
for roll in roll_vals:
for pitch in pitch_vals:
self.gimbal.set_attitude([roll, pitch])
x, X = self.calc_gimbal_camera_view()
gimbal_cam_data.corners2d_ud.append(x)
gimbal_cam_data.corners3d_ud.append(X)
joint_data.append([roll, pitch])
gimbal_cam_data.corners2d_ud = np.array(gimbal_cam_data.corners2d_ud)
gimbal_cam_data.corners3d_ud = np.array(gimbal_cam_data.corners3d_ud)
joint_data = np.array(joint_data)
return [static_cam_data, gimbal_cam_data], joint_data
def test_plot(self):
# Gimbal initial guess
# # -- tau (x, y, z, roll, pitch, yaw)
# tau_s = np.array([-0.045, -0.085, 0.08, 0.0, 0.0, 0.0])
# tau_d = np.array([0.01, 0.0, -0.03, pi / 2.0, 0.0, -pi / 2.0])
# # -- link (theta, alpha, a, d)
# alpha = pi / 2.0
# offset = -pi / 2.0
# roll = 0.0
# pitch = 0.1
# link1 = np.array([offset + roll, alpha, 0.0, 0.045])
# link2 = np.array([pitch, 0.0, 0.0, 0.0])
# # Optimized gimbal params
# offset = -pi / 2.0
# roll = 0.0
# pitch = 0.0
#
# tau_s = np.array([-0.0419, -0.0908, 0.0843, 0.0189, -0.0317, -0.0368])
# tau_d = np.array([0.0032, 0.0007, -0.0302, 1.5800, 0.0123, -1.5695])
# link1 = np.array([offset + roll, 1.5731, -0.0012, 0.0404])
# link2 = np.array([pitch, 0.0, 0.0, 0.0])
# Real 2 initial guess
# offset = -pi / 2.0
# roll = 0.0
# pitch = 0.0
# tau_s = np.array([0.0, -0.08, 0.1143, 0.0, 0.0, 0.0])
# tau_d = np.array([0.0, 0.0, 0.0, pi / 2.0, 0.0, -pi / 2.0])
# link1 = np.array([offset + roll, # theta
# pi / 2.0, # alpha
# 0.0, # a
# 0.0]) # d
# link2 = np.array([pitch, 0.0, 0.0, 0.0])
# Real 2 optimized params
offset = -pi / 2.0
roll = 0.0
pitch = 0.0
tau_s = np.array([
0.01519, -0.036509, 0.096354,
deg2rad(2.15041521),
deg2rad(1.41060977),
deg2rad(18.52822371)
])
link1 = np.array([
offset + roll, # theta
deg2rad(83.57483), # alpha
-0.0050, # a
0.01683
]) # d
link2 = np.array([pitch, 0.0, 0.0, 0.0])
tau_d = np.array([
-0.00029, 0.00138, -0.03656,
deg2rad(84.14281),
deg2rad(0.28527),
deg2rad(-91.17418)
])
# -- Gimbal model
gimbal_model = GimbalModel(tau_s=tau_s,
tau_d=tau_d,
link1=link1,
link2=link2)
# T_ds = gimbal_model.T_ds()
# p_s = np.array([1.0, 0.0, 0.0, 1.0])
# Plot gimbal model
plot = PlotGimbal(gimbal=gimbal_model,
show_static_frame=True,
show_base_frame=True,
show_end_frame=True,
show_dynamic_frame=True)
plot.set_attitude([0.0, 0.0])
plot.plot()
# Plot
debug = True
# debug = False
if debug:
plt.show()