def rigid_inv_transform(p, R, t):
p_ = Point3D(p)
q = np.matmul(R.T, p_.p - t)
if isinstance(p, Point3D):
return Point3D(q)
else:
return q
def run(self):
angle = Point3D(30.0, 30.0, 0.0)
delta = Point3D(0, 0, 0)
friction_factor = 0.98
init_pos = None
dragging = False
while True:
for event in pygame.event.get():
if event.type == pygame.QUIT:
sys.exit()
elif event.type == pygame.MOUSEBUTTONDOWN and event.button == 1:
dragging = True
init_pos = Point2D(*event.pos)
elif event.type == pygame.MOUSEBUTTONUP and event.button == 1:
dragging = False
elif event.type == pygame.MOUSEMOTION and dragging:
curr_pos = Point2D(*event.pos)
delta.y, delta.x = (init_pos -
curr_pos) * 2 * math.pi / self.width
angle += delta
cube = Cube(size=100,
angle_x=angle.x,
angle_y=angle.y,
angle_z=angle.z)
self.__draw_cube(cube)
if not dragging:
delta *= friction_factor
angle += delta
pygame.display.flip()
def _generate_step(self, step_number):
a = self._l_b + Point3D(y=-(step_number * self._step_length))
b = a + Point3D(z=self._step_height * (step_number + 1))
c = b + Point3D(y=-self._step_length)
d = a + Point3D(y=-self._step_length)
f = b + Point3D(z=-self._step_height)
a1 = a + Point3D(x=self._width)
b1 = b + Point3D(x=self._width)
c1 = c + Point3D(x=self._width)
d1 = d + Point3D(x=self._width)
f1 = f + Point3D(x=self._width)
self.triangles.append(Triangle(a, b, c))
self.triangles.append(Triangle(a, d, c))
self.triangles.append(Triangle(a1, b1, c1))
self.triangles.append(Triangle(a1, d1, c1))
self.triangles.append(Triangle(b, f, f1))
self.triangles.append(Triangle(f1, b1, b))
self.triangles.append(Triangle(c, b, b1))
self.triangles.append(Triangle(b1, c1, c))
self.edges.append(Edge(f, b))
self.edges.append(Edge(f1, b1))
self.edges.append(Edge(b, b1))
self.edges.append(Edge(c, c1))
self.edges.append(Edge(b, c))
self.edges.append(Edge(b1, c1))
def rigid_transform(p, R, t):
p_ = Point3D(p)
q = np.matmul(R, p_.p) + t
if isinstance(p, Point3D):
return Point3D(q)
else:
return q
def _generate_back(self):
a = self._l_t + Point3D(z=self._step_count * self._step_height)
b = a + Point3D(self._width)
self.triangles.append(Triangle(a, b, self._r_t))
self.triangles.append(Triangle(a, self._l_t, self._r_t))
self.edges.append(Edge(a, self._l_t))
self.edges.append(Edge(b, self._r_t))
def _generate_base(self):
self._l_t = self._base_point
self._r_t = self._l_t + Point3D(self._width)
self._r_b = self._r_t + Point3D(y=self._step_count * self._step_length)
self._l_b = self._r_b + Point3D(x=-self._width)
self.triangles.append(Triangle(self._l_t, self._r_t, self._r_b))
self.triangles.append(Triangle(self._l_t, self._l_b, self._r_b))
self.edges.append(Edge(self._l_t, self._r_t))
self.edges.append(Edge(self._r_t, self._r_b))
self.edges.append(Edge(self._r_b, self._l_b))
self.edges.append(Edge(self._l_b, self._l_t))
def __init__(self, start_point, width, height, depth):
# from lower to upper plane, clockwise
self.p0 = Point3D(start_point.x, start_point.y, start_point.z)
self.p1 = Point3D(start_point.x, start_point.y, start_point.z + depth)
self.p2 = Point3D(start_point.x + width, start_point.y,
start_point.z + depth)
self.p3 = Point3D(start_point.x + width, start_point.y, start_point.z)
self.p4 = Point3D(start_point.x, start_point.y + height, start_point.z)
self.p5 = Point3D(start_point.x, start_point.y + height,
start_point.z + depth)
self.p6 = Point3D(start_point.x + width, start_point.y + height,
start_point.z + depth)
self.p7 = Point3D(start_point.x + width, start_point.y + height,
start_point.z)
self.points = [
self.p0, self.p1, self.p2, self.p3, self.p4, self.p5, self.p6,
self.p7
]
self.edges = [
Edge(self.p0, self.p1),
Edge(self.p1, self.p2),
Edge(self.p2, self.p3),
Edge(self.p3, self.p0), # down
Edge(self.p0, self.p4),
Edge(self.p1, self.p5),
Edge(self.p2, self.p6),
Edge(self.p3, self.p7), # between
Edge(self.p4, self.p5),
Edge(self.p5, self.p6),
Edge(self.p6, self.p7),
Edge(self.p7, self.p4)
] # up
def generate_rand_points(num=1, loc=[0, 0, 0], scale=[1, 1, 1]):
x = np.random.normal(loc[0], scale[0], (num, ))
y = np.random.normal(loc[1], scale[1], (num, ))
z = np.random.normal(loc[2], scale[2], (num, ))
points = []
for i in range(num):
points.append(Point3D((x[i], y[i], z[i])))
return points
def find_beta_n1(v, ctrl_pw):
ctrl_pc = []
for i in range(0, len(ctrl_pw)):
ctrl_pc.append(Point3D(v[i * 3:i * 3 + 3]))
dist_c = calc_dist(ctrl_pc)
dist_w = calc_dist(ctrl_pw)
scale = calc_sign(ctrl_pc) * np.matmul(dist_w, dist_c) / np.matmul(
dist_c, dist_c)
return np.array([0, 0, 0, scale])
def illum_map_elevation(elevation):
""" Illumination map for specific elevation. """
lamp = Lamp(light, Point3D(0.0, 0.0, LAMP_HEIGHT), LAMP_AZIMUT_DEG,
elevation)
plot_illuminance_map(lamp.illuminance,
x_distances,
y_distances,
color_levels=np.linspace(0, 500, 25),
title=f"Illumination [lux] (elev={elevation})")
def triangulation(n1, n2, p1, p2):
delt = p2 - p1
N1 = np.matmul(n1, n1) * np.eye(3) - np.matmul(n1.reshape(3, 1),
n1.reshape((1, 3)))
N2 = np.matmul(n2, n2) * np.eye(3) - np.matmul(n2.reshape(3, 1),
n2.reshape((1, 3)))
a2 = geo.quadratic_form(n2, N1, delt)
b2 = geo.quadratic_form(n2, N1, n2)
t2 = -a2 / b2
v2 = t2 * n2
P2 = Point3D(p2 + v2)
a1 = geo.quadratic_form(n1, N2, -delt)
b1 = geo.quadratic_form(n1, N2, n1)
t1 = -a1 / b1
v1 = t1 * n1
P1 = Point3D(p1 + v1)
P = (P1 + P2) / 2
return P, P1, P2
def __init__(self, start_point, width, depth, height, color):
self.color = color
p = start_point
self.bottom_0 = Point3D(p.x, p.y, p.z)
self.bottom_1 = Point3D(p.x + width, p.y, p.z)
self.bottom_2 = Point3D(p.x + width, p.y, p.z + depth)
self.bottom_3 = Point3D(p.x, p.y, p.z + depth)
self.top_0 = Point3D(p.x, p.y + height, p.z)
self.top_1 = Point3D(p.x + width, p.y + height, p.z)
self.top_2 = Point3D(p.x + width, p.y + height, p.z + depth)
self.top_3 = Point3D(p.x, p.y + height, p.z + depth)
self.edge_bottom_0 = Edge(self.bottom_0, self.bottom_1)
self.edge_bottom_1 = Edge(self.bottom_1, self.bottom_2)
self.edge_bottom_2 = Edge(self.bottom_2, self.bottom_3)
self.edge_bottom_3 = Edge(self.bottom_3, self.bottom_0)
self.edge_vertical_0 = Edge(self.bottom_0, self.top_0)
self.edge_vertical_1 = Edge(self.bottom_1, self.top_1)
self.edge_vertical_2 = Edge(self.bottom_2, self.top_2)
self.edge_vertical_3 = Edge(self.bottom_3, self.top_3)
self.edge_top_0 = Edge(self.top_0, self.top_1)
self.edge_top_1 = Edge(self.top_1, self.top_2)
self.edge_top_2 = Edge(self.top_2, self.top_3)
self.edge_top_3 = Edge(self.top_3, self.top_0)
self.points = [self.bottom_0, self.bottom_1, self.bottom_2, self.bottom_3,
self.top_0, self.top_1, self.top_2, self.top_3]
self.edges = [
self.edge_bottom_0,
self.edge_bottom_1,
self.edge_bottom_2,
self.edge_bottom_3,
self.edge_vertical_0,
self.edge_vertical_1,
self.edge_vertical_2,
self.edge_vertical_3,
self.edge_top_0,
self.edge_top_1,
self.edge_top_2,
self.edge_top_3
]
self.faces = [
Face([self.edge_bottom_0, self.edge_bottom_1, self.edge_bottom_2, self.edge_bottom_3], color),
Face([self.edge_top_0, self.edge_top_1, self.edge_top_2, self.edge_top_3], color),
Face([self.edge_bottom_0, self.edge_vertical_1, self.edge_top_0.inversed(), self.edge_vertical_0.inversed()], color),
Face([self.edge_bottom_1, self.edge_vertical_2, self.edge_top_1.inversed(), self.edge_vertical_1.inversed()], color),
Face([self.edge_bottom_2, self.edge_vertical_3, self.edge_top_2.inversed(), self.edge_vertical_2.inversed()], color),
Face([self.edge_bottom_3, self.edge_vertical_0, self.edge_top_3.inversed(), self.edge_vertical_3.inversed()], color)
]
def project_image2camera(self, p2d):
"""
back-project the 2d image Point3D to the homogeneous coordinate in PinHoleCamera Frame
p2d: Nx2 array of 2d key points
return: list of homogeneous 3d points of in the PinHoleCamera Frame
"""
p2d = np.column_stack((p2d, np.ones((p2d.shape[0], 1))))
p3d_tmp = np.matmul(self.K_, p2d.T)
p3d = []
for i in range(p3d_tmp.shape[1]):
p3d.append(Point3D(p3d_tmp[:, i]))
return p3d
def estimate_pose_epnp(K, pw, pi, ctrl_num=4):
n = len(pw)
ctrl_pw = get_control_points(pw)
bc = calc_barycentric(ctrl_pw, pw)
L = np.zeros((n * 2, ctrl_num * 3))
fu, fv = K[0, 0], K[1, 1]
uc, vc = K[0, 2], K[1, 2]
for i in range(n):
for j in range(ctrl_num):
L[i * 2, j * 3] = fu * bc[i, j]
L[i * 2, j * 3 + 2] = (uc - pi[i, 0]) * bc[i, j]
L[i * 2 + 1, j * 3 + 1] = fv * bc[i, j]
L[i * 2 + 1, j * 3 + 2] = (vc - pi[i, 1]) * bc[i, j]
u, z, eig_v = np.linalg.svd(L)
eig_v = eig_v.T[:, -4:]
# M = np.matmul(L.T, L)
# lamda, eig_v = np.linalg.eig(M)
# eig_v = eig_v[:, np.argsort(-lamda)][:, -4:]
# set the initial beta for Epnp
data = np.split(eig_v, 4)
target = calc_dist(ctrl_pw)**2
beta_best = find_beta_n1(eig_v[:, -1], ctrl_pw)
solver = EpnpSolver(beta_best, data, target)
err_best = solver.residual
for i in range(2, 5):
beta = find_beta_n234(eig_v[:, -i:], ctrl_pw, N=i)
solver.coef = beta
err = solver.forward()
if err < err_best:
beta_best = beta
err_best = err
solver.coef = beta_best
solver.forward()
# print("error before fine tune: %f" % err_best)
# fine-tune beta
k = 0
while solver.residual > 5e-3 and k < 15:
solver.solve()
k += 1
# print("error after %d iterations: %f" % (k, solver.residual))
# print("error after fine-tune: %f" % solver.residual)
# recover pose
ctrl_pc = [Point3D(np.matmul(pc, solver.coef)) for pc in data]
sign = calc_sign(ctrl_pc)
ctrl_pc = [pc * sign for pc in ctrl_pc]
R, t = geo.recover_Rt(ctrl_pc, ctrl_pw)
return R, t
def show(self, ax, color='blue', s=20):
o = geo.rigid_inv_transform(Point3D((0, 0, 0)), self.R, self.t)
o.plot3d(ax, color=color, s=s)
ex = self.R[0, :]
ey = self.R[1, :]
ez = self.R[2, :]
ax.quiver(o.x,
o.y,
o.z,
ex[0],
ex[1],
ex[2],
normalize=True,
color='red')
ax.quiver(o.x,
o.y,
o.z,
ey[0],
ey[1],
ey[2],
normalize=True,
color='green')
ax.quiver(o.x,
o.y,
o.z,
ez[0],
ez[1],
ez[2],
normalize=True,
color='blue')
sx, sy = [self.f / self.fx, self.f / self.fy]
c = o + ez * self.f
p0 = c - ex * sx * self.img_w / 2 - ey * sy * self.img_h / 2
p1 = c + ex * sx * self.img_w / 2 - ey * sy * self.img_h / 2
p2 = c + ex * sx * self.img_w / 2 + ey * sy * self.img_h / 2
p3 = c - ex * sx * self.img_w / 2 + ey * sy * self.img_h / 2
p0.plot3d(ax, color='red', marker='.', s=20)
p1.plot3d(ax, color=color, marker='.', s=20)
p3.plot3d(ax, color=color, marker='.', s=20)
p2.plot3d(ax, color=color, marker='.', s=20)
ax.plot3D([p0.x, p1.x], [p0.y, p1.y], [p0.z, p1.z], color=color)
ax.plot3D([p1.x, p2.x], [p1.y, p2.y], [p1.z, p2.z], color=color)
ax.plot3D([p2.x, p3.x], [p2.y, p3.y], [p2.z, p3.z], color=color)
ax.plot3D([p3.x, p0.x], [p3.y, p0.y], [p3.z, p0.z], color=color)
ax.plot3D([o.x, p0.x], [o.y, p0.y], [o.z, p0.z], color=color)
ax.plot3D([o.x, p1.x], [o.y, p1.y], [o.z, p1.z], color=color)
ax.plot3D([o.x, p2.x], [o.y, p2.y], [o.z, p2.z], color=color)
ax.plot3D([o.x, p3.x], [o.y, p3.y], [o.z, p3.z], color=color)
def initialize(self):
# (wysokosc, glebokosc, wysokosc)
##popuste plasterki
# self.shapes.append(Cuboid(Point3D(2, -5, 31), 20, 0.2, 20, "green"))
# self.shapes.append(Cuboid(Point3D(2, -7, 31.5), 20, 0.2, 20, "blue"))
# self.shapes.append(Cuboid(Point3D(2, -9, 32), 20, 0.2, 20, "red"))
# self.shapes.append(Cuboid(Point3D(2, -11, 32.5), 20, 0.2, 20, "yellow"))
#oplasterki
self.shapes.append(Cuboid(Point3D(2, -5, 31), 20, 0.2, 20, "green"))
self.shapes.append(Cuboid(Point3D(2, -5, 31.5), 20, 0.2, 20, "blue"))
self.shapes.append(Cuboid(Point3D(2, -5, 32), 20, 0.2, 20, "red"))
self.shapes.append(Cuboid(Point3D(2, -5, 32.5), 20, 0.2, 20, "yellow"))
self.shapes.append(Cuboid(Point3D(2, -5, 33), 20, 0.2, 20, "green"))
self.shapes.append(Cuboid(Point3D(2, -5, 33.5), 20, 0.2, 20, "blue"))
self.shapes.append(Cuboid(Point3D(2, -5, 34), 20, 0.2, 20, "red"))
self.shapes.append(Cuboid(Point3D(2, -5, 34.5), 20, 0.2, 20, "yellow"))
def __init__(self,
light: Light,
position: Point3D = Point3D(0.0, 0.0, 0.0),
azimut_deg: float = 0.0,
elevation_deg: float = 0.0):
"""
:param position: [m]
:param azimut [deg] - light azimut counterclockwise from light front direction
(-90 .. right, 0 .. front, 90 .. left)"
:param elevation [deg] - light elevation
(0 - standard mounting, positive - lighten longer distance, negative - shorter)
"""
self._light = light
self._position = position
self._azimut_deg = azimut_deg
self._elevation_deg = elevation_deg
def __init__(self, master):
self.width = 800
self.height = 800
self.move_step = 2
self.zoom_step = 5
self.rotate_step = math.pi / 18
self.canvas = tk.Canvas(master, width=self.width, height=self.height, bg="black")
self.canvas.pack()
self.painter = Painter(self)
self.camera = Point3D(0, 0, 0)
self.d = 200
self.shapes = []
self.initialize()
self.draw()
def show_projection(self, ax, pw):
pi, pc = self.project_world2image(pw)
for i, p in enumerate(pw):
o = geo.rigid_inv_transform(Point3D((0, 0, 0)), self.R, self.t)
x = pc[i] / pc[i].z
xi = pi[i]
p.plot3d(ax, s=10, marker='o', color='blue')
if not self.is_out_of_bound(xi):
ax.plot3D([o.x, p.x], [o.y, p.y], [o.z, p.z],
color='blue',
linestyle='--',
linewidth=1)
xw = geo.rigid_inv_transform(x, self.R, self.t)
xw.plot3d(ax, s=10, marker='o', color='red')
else:
ax.plot3D([o.x, p.x], [o.y, p.y], [o.z, p.z],
color='red',
linestyle='--',
linewidth=1)
def recover_Rt(pc, pw):
n = len(pc)
center_c = Point3D((0, 0, 0))
center_w = center_c
for p, q in zip(pc, pw):
center_c += p
center_w += q
center_c /= n
center_w /= n
mc = list2mat(pc)
mw = list2mat(pw)
mc_c = mc - np.tile(center_c.p, (n, 1))
mw_c = mw - np.tile(center_w.p, (n, 1))
R = np.matmul(np.linalg.pinv(np.matmul(mw_c.T, mw_c)),
np.matmul(mw_c.T, mc_c))
U, Z, V = np.linalg.svd(R)
R = np.matmul(U, V)
ta = mc - np.matmul(mw, R)
t = np.mean(ta, axis=0)
return R.T, t
def test_camera_func():
import matplotlib.pyplot as plt
plt.figure()
ax = plt.gca(projection='3d')
p1 = Point3D([2, 6, 7])
p1.plot3d(ax, s=5)
R = geo.rodriguez((1, 1, 1), np.pi / 4)
camera1 = PinHoleCamera(R=R)
camera2 = PinHoleCamera(R=R, t=np.array((-4, -4, 0)))
camera1.show(ax)
camera2.show(ax)
pi1, _ = camera1.project_world2image([p1])
pi2, _ = camera2.project_world2image([p1])
pw, _, _ = camera_triangulation(camera1, camera2, pi1, pi2)
print(pw)
# pi = np.array([[1920, 1080]])
# pc = camera1.project_image2camera(pi)
# pi_ = camera1.project_camera2image(pc)
# print(pi_)
# print(pc)
#
# pw = camera1.project_camera2world(pc)
# pc_ = camera1.project_world2camera(pw)
# print(pc_)
# pw[0].plot3d(ax, s=5, marker='x', color='red')
# camera1.show_projection(ax, [p1])
# img = camera1.get_projected_img([p1])
# plt.figure()
# plt.imshow(img)
ax.set_xlim([-3, 10])
ax.set_ylim([-3, 10])
ax.set_zlim([-3, 10])
ax.set_xlabel("x")
ax.set_ylabel("y")
ax.set_zlabel("z")
plt.show()
def __init__(self, step_height, width, step_count):
if step_count < 1:
raise Exception('Step count must be more than 0')
self.triangles = []
self.edges = []
self.x_angle = 0
self.y_angle = 0
self.z_angle = 0
self.dx = 0
self.dy = 0
self.dz = 0
self.scale = 1
self._step_height = step_height
self._step_length = step_height * 2
self._width = width
self._step_count = step_count
self._base_point = Point3D()
self._generate()
def camera_triangulation(camera1, camera2, pi1, pi2, filter=False):
"""
get the back-projected 3D points from the 2D key-points in two views
camera1 & camera2: the two cameras
pi1 & pi2: Nx2 array which stores the 2D key-points of the two cameras
filter: ignore points which have small view angles when filter is True
return: reconstructed points' coordinates in world-frame
"""
assert pi1.shape == pi2.shape
pc1 = camera1.project_image2camera(pi1)
pc2 = camera2.project_image2camera(pi2)
r = np.matmul(camera2.R, camera1.R.T)
a = np.matmul(r, camera1.t) - camera2.t
pw = []
for p, q in zip(pc1, pc2):
b = geo.cross_mat(q.p)
x = np.matmul(np.matmul(b, r), p.p)
y = np.matmul(b, a)
s1 = np.matmul(x, y) / np.matmul(x, x)
pw.append(Point3D(np.matmul(camera1.R.T, s1 * p.p - camera1.t)))
return pw, None, None
def place_a_camera(p,
ez,
ex,
f=1.0,
fx=500,
fy=500,
img_w=1920,
img_h=1080):
ez /= np.linalg.norm(ez)
ex = ex - np.matmul(ez, ex) * ez
ex /= np.linalg.norm(ex)
ey = np.cross(ez, ex)
R = np.row_stack((ex, ey, ez))
t = -np.matmul(R, Point3D(p).p)
return PinHoleCamera(R=R,
t=t,
f=f,
fx=fx,
fy=fy,
img_w=img_w,
img_h=img_h)
def calc_gravity_center(self):
center = Point3D(0, 0, 0)
for point in self.unique_points():
center = center.add_point(point)
return center.divide_by_scalar(len(self.unique_points()))
def test_plot_map():
pw = [Point3D((0, 0, 0)), Point3D((0, 0, 10)), Point3D((20, 0, 0))]
# plot_map(pw, [])
plt.show()
def initialize(self):
self.shapes.append(Cuboid(Point3D(2, -5, 20), 10, 10, 10, "red"))
self.shapes.append(Cuboid(Point3D(-12, -5, 20), 10, 10, 10, "blue"))
self.shapes.append(Cuboid(Point3D(2, -5, 32), 10, 10, 10, "green"))
self.shapes.append(Cuboid(Point3D(-12, -5, 32), 10, 10, 10, "yellow"))
def test_save_ply():
file_name = "../data/test.ply"
save_to_ply([Point3D((0, 0, 0)), Point3D((0, 1, 0))], file_name)
from datetime import time
from actuator import Actuator
import sys
from point import Point3D
import math
actuator = Actuator(['y', [80, 0., 0.], 'z', [80, 0., 0.], 'z', [120, 0., 0.]])
args = sys.argv
angles = actuator.inverse_kinematics(
Point3D(int(args[1]), int(args[2]), int(args[3])))
for i, angle in enumerate(angles):
angles[i] = round(angle, 2)
sys.stdout.write("{0} {1} {2}".format(angles[0], angles[1], angles[2]))
def calc_center(plist):
center = Point3D((0, 0, 0))
for p in plist:
center += p
center = center / len(plist)
return center
