def proyection(points, plane_point, plane_normal):
n = plane_normal / np.linalg.norm(plane_normal)
d = points - plane_point
dist = d.dot(n)
return points - np.outer(dist, n)
def plane_axes(plane_normal):
n = plane_normal / np.linalg.norm(plane_normal)
arb = np.array([1, 0, 0]) if abs(n[0]) < 0.9 else np.array([0, 1, 0])
u = np.cross(n, arb)
u /= np.linalg.norm(u)
v = np.cross(n, u)
v /= np.linalg.norm(v)
return u, v
def sensor_reading(points, plane_point, plane_normal, noise_std=0.5):
proj_3d = proyection(points, plane_point, plane_normal)
u, v = plane_axes(plane_normal)
proj_vectors = proj_3d - plane_point
s = proj_vectors.dot(u) + np.random.normal(0, noise_std, len(points))
t = proj_vectors.dot(v) + np.random.normal(0, noise_std, len(points))
return s, t, proj_3d, u, v
def plane_mesh(plane_point, plane_normal, size=12, res=20):
u, v = plane_axes(plane_normal)
lin = np.linspace(-size, size, res)
uu, vv = np.meshgrid(lin, lin)
X = plane_point[0] + uu * u[0] + vv * v[0]
Y = plane_point[1] + uu * u[1] + vv * v[1]
Z = plane_point[2] + uu * u[2] + vv * v[2]
return X, Y, Z
sensors = [
dict(point=np.array([5., -6., 0.]), normal=np.array([1., -1., 0.]),
color='royalblue', name='Sensor A'),
dict(point=np.array([10., 0., 0.]), normal=np.array([10., 0., 0.1]),
color='seagreen', name='Sensor B'),
dict(point=np.array([3., 3., 3.]), normal=np.array([1., 1., 1.]),
color='darkorange', name='Sensor C'),
]
readings = []
for s in sensors:
sc, tc, proj3d, u, v = sensor_reading(
points, s['point'], s['normal'], noise_std=0.5
)
readings.append({'s': sc, 't': tc, 'proj3d': proj3d,
'u': u, 'v': v, **s})