Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
random_state = np.random.RandomState(42)
PA = np.ones((10, 4))
PA[:, :3] = 0.1 * random_state.randn(10, 3)
PA[:, 0] += 0.3
PA[:, :3] += 0.3
x_translation = -0.1
y_translation = 0.2
z_rotation = np.pi / 4.0
A2B = np.array([
[np.cos(z_rotation), -np.sin(z_rotation), 0.0, x_translation],
[np.sin(z_rotation), np.cos(z_rotation), 0.0, y_translation],
[0.0, 0.0, 1.0, 0.0],
[0.0, 0.0, 0.0, 1.0]
])
PB = transform(A2B, PA)
plot_transform(ax=ax, A2B=np.eye(4))
ax.scatter(PA[:, 0], PA[:, 1], PA[:, 2], c="orange")
plot_transform(ax=ax, A2B=A2B, ls="--", alpha=0.5)
ax.scatter(PB[:, 0], PB[:, 1], PB[:, 2], c="cyan")
axis_arrow = Arrow3D(
[0.7, 0.3],
[0.4, 0.9],
[0.2, 0.2],
mutation_scale=20, lw=3, arrowstyle="-|>", color="k")
ax.add_artist(axis_arrow)
plt.tight_layout()
plt.show()
focal_length : float
Focal length of the camera
image_center : array-like, shape (2,), optional (default: image_size / 2)
Center of the image
kappa : float, optional (default: 0)
TODO document
Returns
-------
P_img : array-like, shape (n_points, 2)
Points on image
"""
world2cam = invert_transform(cam2world)
P_cam = transform(world2cam, P_world)
P_sensor = cam2sensor(P_cam, focal_length, kappa)
P_img = sensor2img(P_sensor, sensor_size, image_size, image_center)
return P_img
def plot(self, tm, frame, ax=None, color="k", wireframe=True):
A2B = tm.get_transform(self.frame, frame)
corners = np.array([
[0, 0, 0],
[0, 0, 1],
[0, 1, 0],
[0, 1, 1],
[1, 0, 0],
[1, 0, 1],
[1, 1, 0],
[1, 1, 1]
])
corners = (corners - 0.5) * self.size
corners = transform(
A2B, np.hstack((corners, np.ones((len(corners), 1)))))[:, :3]
for i, j in [(0, 1), (0, 2), (1, 3), (2, 3),
(4, 5), (4, 6), (5, 7), (6, 7),
(0, 4), (1, 5), (2, 6), (3, 7)]:
ax.plot([corners[i, 0], corners[j, 0]],
[corners[i, 1], corners[j, 1]],
[corners[i, 2], corners[j, 2]], c=color)
return ax
import numpy as np
import matplotlib.pyplot as plt
import pytransform3d.rotations as pyrot
import pytransform3d.transformations as pytr
p = np.array([0.0, 0.0, -0.5])
a = np.array([0.0, 0.0, 1.0, np.pi])
B2A = pytr.transform_from(pyrot.matrix_from_axis_angle(a), p)
p = np.array([0.3, 0.4, 0.5])
a = np.array([0.0, 0.0, 1.0, -np.pi / 2.0])
C2B = pytr.transform_from(pyrot.matrix_from_axis_angle(a), p)
C2A = pytr.concat(C2B, B2A)
p = pytr.transform(C2A, np.ones(4))
ax = pytr.plot_transform(A2B=B2A)
pytr.plot_transform(ax, A2B=C2A)
ax.scatter(p[0], p[1], p[2])
plt.show()