def create_cam_distribution(cam=None,
plane_size=(0.3, 0.3),
theta_params=(0, 360, 10),
phi_params=(0, 70, 5),
r_params=(0.25, 1.0, 4),
plot=False):
if cam == None:
# Create an initial camera on the center of the world
cam = Camera()
f = 800
cam.set_K(fx=f, fy=f, cx=320, cy=240) #Camera Matrix
cam.img_width = 320 * 2
cam.img_height = 240 * 2
# we create a default plane with 4 points with a side lenght of w (meters)
plane = Plane(origin=np.array([0, 0, 0]),
normal=np.array([0, 0, 1]),
size=plane_size,
n=(2, 2))
#We extend the size of this plane to account for the deviation from a uniform pattern
#plane.size = (plane.size[0] + deviation, plane.size[1] + deviation)
d_space = np.linspace(r_params[0], r_params[1], r_params[2])
t_list = []
for d in d_space:
xx, yy, zz = uniform_sphere(theta_params, phi_params, d, False)
sphere_points = np.array(
[xx.ravel(), yy.ravel(), zz.ravel()], dtype=np.float32)
t_list.append(sphere_points)
t_space = np.hstack(t_list)
cams = []
for t in t_space.T:
cam = cam.clone()
cam.set_t(-t[0], -t[1], -t[2])
cam.set_R_mat(R_matrix_from_euler_t(0.0, 0, 0))
cam.look_at([0, 0, 0])
plane.set_origin(np.array([0, 0, 0]))
plane.uniform()
objectPoints = plane.get_points()
imagePoints = cam.project(objectPoints)
#if plot:
# cam.plot_image(imagePoints)
if ((imagePoints[0, :] < cam.img_width) &
(imagePoints[0, :] > 0)).all():
if ((imagePoints[1, :] < cam.img_height) &
(imagePoints[1, :] > 0)).all():
cams.append(cam)
if plot:
planes = []
plane.uniform()
planes.append(plane)
plot3D(cams, planes)
return cams
def create_plane_distribution(plane_size=(0.3, 0.3),
theta_params=(0, 360, 5),
phi_params=(0, 70, 3),
r_params=(0.5, 1.0, 2),
plot=False):
# We extend the size of this plane to account for the deviation from a uniform pattern
# plane.size = (plane.size[0] + deviation, plane.size[1] + deviation)
planes = []
d_space = np.linspace(r_params[0], r_params[1], r_params[2])
t_list = []
for d in d_space:
xx, yy, zz = uniform_sphere(theta_params, phi_params, d, False)
sphere_points = np.array(
[xx.ravel(), yy.ravel(), zz.ravel()], dtype=np.float32)
t_list.append(sphere_points)
t_space = np.hstack(t_list)
# print t_space.shape
for t in t_space.T:
# We set one static plane at (0,0,0) in the world coordinate, this static plane is fixed!!!
# We set a static camera straight up of this static plane, this camera is also fixed!!!
# The relationship between static plane, static camera and new plane is : T = T1 * T2
# we create a default plane with 4 points with a side lenght of w (meters)
# The origin of each new plane is the point of sphere at each position
real_origin = t
plane = Plane(origin=real_origin,
normal=np.array([0, 0, 1]),
size=plane_size,
n=(2, 2))
plane.uniform() # TODO
planes.append(plane)
# print len(planes)
if plot:
height = t_space[2]
plot3D(planes, height)
return planes
0.,
0.,
], [
1.,
1.,
1.,
1.,
]])
## CREATE A SET OF IMAGE POINTS FOR VALIDATION OF THE HOMOGRAPHY ESTIMATION
# This will create a grid of 16 points of size = (0.3,0.3) meters
validation_plane = Plane(origin=np.array([0, 0, 0]),
normal=np.array([0, 0, 1]),
size=(0.3, 0.3),
n=(4, 4))
validation_plane.uniform()
## we create the gradient for the point distribution
normalize = False
n = 0.00000004 #condition number norm 4 points
gradient = gd.create_gradient(metric='condition_number', n=n)
#define the plots
#one Figure for image and object points
fig11 = plt.figure('Image Plane Coordinates')
ax_image = fig11.add_subplot(111)
fig12 = plt.figure('Control Points plane Coordinates')
ax_object = fig12.add_subplot(111)
if calc_metrics:
#another figure for Homography error and condition numbers
import Rt_matrix_from_euler_zyx as R_matrix_from_euler_zyx
import Rt_matrix_from_euler_t as Rt_matrix_from_euler_t
from solve_pnp import pose_pnp
import cv2
import error_functions as ef
import CondNumTheoryValidation.hT_gradient as gd
# ----------------------- Basic Infos ---------------------------------------------------
# ----------------------- Marker object points -----------------------------------------
plane_size = (0.3, 0.3)
plane = Plane(origin=np.array([0, 0, 0]),
normal=np.array([0, 0, 1]),
size=plane_size,
n=(2, 2))
plane.set_origin(np.array([0, 0, 0]))
plane.uniform()
objectPoints = plane.get_points()
new_objectPoints = np.copy(objectPoints)
# --------------------------------------------------------------------------------------
def cellCenterPosition(path, grid_reso):
"""
Get the exact position of each cell in the real world
Based on Real World Coordinate System [30,60]
:param pos:
:param grid_step:
:return:
"""
real_path = np.eye(2, 1, dtype=float)
length = path.shape[1]