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 getT_MC_and_Rt_errors(T_WM, pos_world, Rmat_error_loop, tvec_error_loop):
pos_world_homo = np.array([pos_world[0], pos_world[1], 0, 1])
pos_marker = np.dot(T_WM, pos_world_homo)
# print "pos_marker\n", pos_marker
# 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
cam = cam.clone()
cam.set_t(pos_marker[0], pos_marker[1], pos_marker[2], 'world')
cam.set_R_mat(Rt_matrix_from_euler_t.R_matrix_from_euler_t(0.0, 0, 0))
# print "cam.R\n",cam.R
cam.look_at([0, 0, 0])
# print "cam.Rt after look at\n",cam.R
objectPoints = np.copy(new_objectPoints)
imagePoints = np.array(cam.project(objectPoints, False))
new_imagePoints = np.copy(imagePoints)
# -----------------------------------------------------------------
new_imagePoints_noisy = cam.addnoise_imagePoints(new_imagePoints,
mean=0,
sd=4)
# print "new_imagePoints_noisy\n",new_imagePoints_noisy
debug = False
# TODO cv2.SOLVEPNP_DLS, cv2.SOLVEPNP_EPNP, cv2.SOLVEPNP_ITERATIVE
pnp_tvec, pnp_rmat = pose_pnp(new_objectPoints, new_imagePoints_noisy,
cam.K, debug, cv2.SOLVEPNP_ITERATIVE, False)
# Calculate errors
Cam_clone_cv2 = cam.clone_withPose(pnp_tvec, pnp_rmat)
tvec_error, Rmat_error = ef.calc_estimated_pose_error(
cam.get_tvec(), cam.R, Cam_clone_cv2.get_tvec(), pnp_rmat)
Rmat_error_loop.append(Rmat_error)
tvec_error_loop.append(tvec_error)
# print "cam.get_world_position()\n",cam.get_world_position()
t = np.eye(4)
t[:3, 3] = pnp_tvec[:3]
T_MC = np.dot(t, pnp_rmat)
return T_MC
from vision.camera import Camera from vision.plane import Plane from vision.screen import Screen import numpy as np import matplotlib.pyplot as plt from mayavi import mlab import cv2 #%% Create a camera cam = Camera() #Set camera intrinsic parameters cam.set_K(794, 781, 640, 480) cam.img_height = 1280 cam.img_width = 960 #%% #Create 1 Screen s1 = Screen() s1.set_origin(np.array([0.0, 0, 0])) s1.set_dimensions(0.50, 0.30) s1.set_pixel_pitch(0.08) red = (1, 0, 0) s1.set_color(red) s1.curvature_radius = 1.800 s1.update() s1.rotate_x(deg2rad(-190.)) #s1.rotate_y(deg2rad(-15.))