In [122]:
import numpy as np
import matplotlib.pyplot as plt
from math import sin, cos, tan, atan, asin, pi, hypot
from pylab import imread
from mpl_toolkits.mplot3d import Axes3D
from filter import findPoints, filterNoise, substract
Deux lasers sont placés de part et d'autre de la caméra, leurs faisceaux verticaux forment une ligne au centre du champ de vision de la caméra (plan de l'image) à une distance de $ L $ de l'objectif. La caméra est à une hauteur $ H_c $ et le plateau à une hauteur $H_p$, et cette différence de hauteur donne à la caméra un angle de plongée $\delta$. L'angle orienté entre le laser gauche et le plan de l'image est noté $ \gamma_G = 78° $, et celui du laser droit $ \gamma_D = 83° $. L'angle d'ouverture de la caméra à mi-hauteur du plan de l'image est $ \alpha = 60°$.
Les photos sont numerotées. La photo 2n.png paire a été prise avec les lasers allumés, la photo 2n+1.png (impaire) avec les lasers éteint.
In [123]:
# Numéro de la photo à analyser
PHOTO_NUM = 14
# Chemin des images
PHOTO_ON, PHOTO_OFF = "imgs/%02d.png"%(2*PHOTO_NUM), "imgs/%02d.png"%(2*PHOTO_NUM+1)
image = filterNoise(substract(PHOTO_ON, PHOTO_OFF))
def deg2rad(x): return pi*float(x)/180
def rad2deg(x): return 180*float(x)/pi
# Paramètres mesurés
L = 53.2
H_C = 39
H_P = 18.5
GAMMA_D = -deg2rad(83)
GAMMA_G = deg2rad(78)
ALPHA = deg2rad(60)
# Paramètres de calibration: coordonnées en pixels du centre du plateau
CX, CY = 317, 310
# Paramètres calculés
H_RELATIVE = H_C - H_P
DELTA = asin(H_RELATIVE/L)
LASER_G = np.array([L * tan(pi/2-GAMMA_G), 0, 0])
LASER_D = np.array([L * tan(pi/2-GAMMA_D), 0, 0])
CAM = np.array([0, -L, H_RELATIVE])
On cherche à placer les points dans un repère centré sur le centre du plateau.
Si un point est détecté à la position $\begin{pmatrix}i\\j\end{pmatrix}$ dans l'image, on peut lui associer un rayon sortant de la caméra, ayant pour équation $\zeta \begin{pmatrix}\sin\theta\\\cos\theta\\\sin(\phi-\delta)\end{pmatrix}$, où $\theta$ est l'angle horizontal, et $\phi$ l'angle vertical par rapport au centre de l'image. Ce rayon croise le plan du laser qui a pour équation
$\lambda \begin{pmatrix}\cos\gamma\\\sin\gamma\\0\end{pmatrix} + \mu \begin{pmatrix}0\\0\\1\end{pmatrix}$
L'écart entre le plan décrit par le laser et la caméra peut être déterminé avec $\gamma$ et $L$: $\begin{pmatrix}L \tan(\frac{\pi}{2}-\gamma) \\ 0 \\0\end{pmatrix}$
Il suffit donc de résoudre le système
$\begin{pmatrix}\cos\gamma&0&\sin\theta \\ \sin\gamma&0&\cos\theta \\ 0&1&\sin(\phi-\delta)\end{pmatrix} \begin{pmatrix}\lambda\\\mu\\\zeta\end{pmatrix} = -\begin{pmatrix}L \tan(\frac{\pi}{2}-\gamma) \\ 0\\0 \end{pmatrix}$
et nous pouvons obtenir la position du point à partir des paramètres ainsi déterminés.
In [124]:
def position(laser, gamma, theta, phi):
"""
laser: position (x,y,z) du laser par rapport à la camera
gamma: angle que fait le laser avec le plan ortogonal à la caméra
theta: angle horizontal du rayon de la camera
phi : angle vertical du rayon de la camera
"""
# vecteur directeur du rayon sortant de la camera
ray = np.array([sin(theta), cos(theta), sin(phi-DELTA)])
# Matrice tq (matrix) * (l, m, z) = (laser)
matrix = np.array([
[cos(gamma), 0, sin(theta)],
[sin(gamma), 0, cos(theta)],
[ 0, 1, sin(phi-DELTA)]
])
l, m, z = np.linalg.solve(matrix, -laser)
return CAM + z * ray
def theta_phi(alpha, image_shape, position):
x, y = map(float, position)
w, h = map(float, image_shape)
ratio = w/h
beta = alpha / ratio
theta = (x - CX)/(w/2) * alpha
phi = (CY - y)/(h/2) * beta
return theta, phi
In [125]:
print map(lambda x: round(x, 1) , position(LASER_G, GAMMA_G, 0, 0))
print map(lambda x: round(x, 1) , position(LASER_D, GAMMA_D, 0, 0))
# Devraient être (0, 0, 0): rayon au centre de la cam
print theta_phi(ALPHA, [480, 640], [0, 0])
print theta_phi(ALPHA, [480, 640], [639, 0])
print theta_phi(ALPHA, [480, 640], [0, 479])
print theta_phi(ALPHA, [480, 640], [639, 479])
In [126]:
XYZ = []
IJ = []
H, W = image.shape[:2]
for j, i in findPoints(image):
IJ.append((j, i))
theta, phi = theta_phi(ALPHA, [W, H], [j, i])
gamma = GAMMA_G if theta < 0 else GAMMA_D
laser = LASER_G if theta < 0 else LASER_D
XYZ.append(position(laser, gamma, theta, phi))
# Coordonnées des points en 3D
X, Y, Z = map(np.array, zip(*XYZ))
# Coordonnées des points sur l'image
I, J = map(np.array, zip(*IJ))
In [127]:
fig = plt.figure(figsize=(15, 15), dpi=80)
R = 25
disk = [(x, y, 0) for x in np.linspace(-R, R) for y in np.linspace(-R, R) if hypot(x, y) <= R]
ax = fig.add_subplot(221, projection='3d')
ax.scatter(X, Y, Z)
rayX, rayY, rayZ = zip(CAM, position(LASER_G, GAMMA_G, 0, 0))
ax.plot(rayX, rayY, rayZ, color='y')
diskX, diskY, diskZ = zip(*disk)
ax.plot(diskX, diskY, diskZ, '.', color='g', alpha=0.25)
plt.xlim(-L, L)
plt.ylim(-L, L)
photo = imread(PHOTO_ON)
ax = fig.add_subplot(222)
h, w = photo.shape[:2]
ax.imshow(photo)
ax.scatter(I, J)
ax = fig.add_subplot(223)
h, w = photo.shape[:2]
ax.imshow(photo)
ax.plot([CX, CX], [0, h], 'y')
ax.plot([0, w], [CY, CY], 'y')
photo = imread(PHOTO_OFF)
ax = fig.add_subplot(224)
h, w = photo.shape[:2]
ax.imshow(photo)
ax.plot([CX, CX], [0, h], 'y')
ax.plot([0, w], [CY, CY], 'y')
plt.show()
In [ ]:
plt.scatter?
In [ ]: