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 __init__(self,
width=0.5184,
height=0.324,
diagonal_size=0.61,
pixel_pitch=0.270,
resolution=(1920.1200),
aspect_ratio=(16, 10),
curvature_radius=4.0):
#In meters
self.width = width
self.height = height
self.diagonal_size = diagonal_size
#In millimeters
self.pixel_pitch = pixel_pitch
#In pixels
self.resolution = resolution
#unitless
self.aspect_ratio = aspect_ratio
#curvature radius in meters | a value of 0.0 means a plane
self.curvature_radius = curvature_radius
Plane.__init__(self)
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.015, -0.12, -0.09, -0.09], [
0.,
0.,
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)
# theta_params = (0,360,2), phi_params = (0,70,2),
# r_params = (0.5,2.0,2), plot=False)
cams = create_cam_distribution(cam,
plane_size,
theta_params=(0, 360, 10),
phi_params=(0, 70, 10),
r_params=(0.3, 2.0, 5),
plot=False)
print len(cams)
#%%
new_objectPoints = pl.get_points()
## 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.000001 #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 and Object Coordinates')
ax_image = fig11.add_subplot(121)
ax_object = fig11.add_subplot(122)
if calc_metrics:
## CREATE A SIMULATED CAMERA
cam = Camera()
cam.set_K(fx=800, fy=800, cx=640, cy=480)
cam.set_width_heigth(1280, 960)
## DEFINE CAMERA POSE LOOKING STRAIGTH DOWN INTO THE PLANE MODEL
#cam.set_R_axisAngle(1.0, 0.0, 0.0, np.deg2rad(179.0))
#cam.set_t(0.0,-0.0,0.5, frame='world')
cam.set_R_axisAngle(1.0, 0.0, 0.0, np.deg2rad(140.0))
cam.set_t(0.0, -0.4, 0.4, frame='world')
## Define a Display plane
pl = Plane(origin=np.array([0, 0, 0]),
normal=np.array([0, 0, 1]),
size=(0.3 + 0.5, 0.2 + 0.5),
n=(2, 2))
pl.random(n=6, r=0.001, min_sep=0.001)
## CREATE A SET OF IMAGE POINTS FOR VALIDATION OF THE HOMOGRAPHY ESTIMATION
validation_plane = Plane(origin=np.array([0, 0, 0]),
normal=np.array([0, 0, 1]),
size=(0.5, 0.5),
n=(4, 4))
validation_plane.uniform()
## we create the gradient for the point distribution
gradient = gd6.create_gradient(metric='condition_number')
#gradient = gd6.create_gradient(metric='volker_metric')
#gradient = gd6.create_gradient(metric='pnorm_condition_number')
from solve_ippe import pose_ippe_both
from solve_pnp import pose_pnp
import cv2
import math
import decimal
calc_metrics = False
number_of_points = 4
## CREATE A SIMULATED CAMERA
cam = Camera()
cam.set_K(fx=800, fy=800, cx=640 / 2., cy=480 / 2.)
cam.set_width_heigth(640, 480)
## 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()
# ---------This plane should be same to the plane in cam distribution!!!!-----------------------
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)
# print "new_objectPoints",new_objectPoints
# -------------------------------------------------------------------------------------------------
normalized = True
import decimal
from scipy.linalg import expm, rq, det, inv
from vision.camera import Camera
from vision.plane import Plane
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:
#cam.set_t(0.0,-1,1.0, frame='world')
#
#r = 0.5
#angle = 10
#x = r*np.cos(np.deg2rad(angle))
#z = r*np.sin(np.deg2rad(angle))
#cam.set_t(0, x,z)
#cam.set_R_mat(R_matrix_from_euler_t(0.0,0,0))
#cam.look_at([0,0,0])
#cam.set_R_axisAngle(1.0, 0.0, 0.0, np.deg2rad(110.0))
#cam.set_t(0.0,-0.3,0.1, frame='world')
## Define a Display plane
pl = Plane(origin=np.array([0, 0, 0]),
normal=np.array([0, 0, 1]),
size=(0.3, 0.3),
n=(2, 2))
pl = CircularPlane()
pl.random(n=number_of_points, r=0.01, min_sep=0.01)
objectPoints = pl.get_points()
x1, y1, x2, y2, x3, y3, x4, y4 = gd.extract_objectpoints_vars(objectPoints)
imagePoints_true = np.array(cam.project(objectPoints, False))
imagePoints_measured = cam.addnoise_imagePoints(imagePoints_true, mean=0, sd=4)
repro = gd.repro_error_autograd(x1, y1, x2, y2, x3, y3, x4, y4, cam.P,
imagePoints_measured)
#%%
## CREATE A SET OF IMAGE POINTS FOR VALIDATION OF THE HOMOGRAPHY ESTIMATION
validation_plane = Plane(origin=np.array([0, 0, 0]),
def create_cam_distribution(cam=None, plane=None, deviation=0, 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
if plane == None:
# we create a default plane with 4 points with a side lenght of w (meters)
w = 0.17
plane = Plane(origin=np.array([0, 0, 0]),
normal=np.array([0, 0, 1]),
size=(w, w),
n=(2, 2))
else:
if deviation > 0:
#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)
# We create an uniform distribution of points in image coordinates
x_min = 0
x_max = cam.img_width
y_min = 0
y_max = cam.img_height
x_dist = np.linspace(x_min, x_max, 3)
y_dist = np.linspace(y_min, y_max, 3)
xx, yy = np.meshgrid(x_dist, y_dist)
hh = np.ones_like(xx, dtype=np.float32)
imagePoints = np.array(
[xx.ravel(), yy.ravel(), hh.ravel()], dtype=np.float32)
# Backproject the pixels into rays (unit vector with the tail at the camera center)
Kinv = np.linalg.inv(cam.K)
unit_rays = np.array(np.dot(Kinv, imagePoints))
#fig = plt.figure()
#ax = fig.add_subplot(111, projection='3d')
#origin = np.zeros_like(unit_rays[0,:])
#ax.quiver(origin,origin,origin,unit_rays[0,:],unit_rays[1,:],unit_rays[2,:], length=1.0, pivot = 'tail')
#ax.scatter(unit_rays[0,:],unit_rays[1,:],unit_rays[2,:], 'rx')
#Select a linear space of distances based on focal length (like in IPPE paper)
#d_space = np.linspace(f/2,2*f, 4)
#Select a linear space of distances based on focal length (like in IPPE paper)
d_space = np.linspace(0.25, 1.0, 4)
#t = d*unit_rays;
t_list = []
for d in d_space:
#t_list.append(d*unit_rays)
xx, yy, zz = uniform_sphere((0, 360), (0, 80), d, 4, 4, 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)
#we now create a plane model for each t
pl_space = []
for t in t_space.T:
pl = plane.clone()
pl.set_origin(np.array([t[0], t[1], t[2]]))
pl.uniform()
pl_space.append(pl)
#ax.scatter(t_space[0,:],t_space[1,:],t_space[2,:], color = 'b')
for pl in pl_space:
objectPoints = pl.get_points()
#ax.scatter(objectPoints[0,:],objectPoints[1,:],objectPoints[2,:], color = 'r')
cams = []
for pl in pl_space:
cam = cam.clone()
cam.set_t(-pl.origin[0], -pl.origin[1], -pl.origin[2])
cam.set_R_mat(R_matrix_from_euler_t(0.0, 0, 0))
cam.look_at([0, 0, 0])
#
pl.set_origin(np.array([0, 0, 0]))
pl.uniform()
objectPoints = pl.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 = []
pl.uniform()
planes.append(pl)
plot3D(cams, planes)
return cams
cy = 480
cam.set_K(fx, fy, cx, cy)
cam.img_width = 1280
cam.img_height = 960
## DEFINE CAMERA POSE LOOKING STRAIGTH DOWN INTO THE PLANE MODEL
cam.set_R_axisAngle(1.0, 1.0, 0.0, np.deg2rad(165.0))
cam_world = np.array([0.0, -0.2, 1, 1]).T
cam_t = np.dot(cam.R, -cam_world)
cam.set_t(cam_t[0], cam_t[1], cam_t[2])
cam.set_P()
H_cam = cam.homography_from_Rt()
#Create a plane with 4 points to start
pl = Plane(origin=np.array([0, 0, 0]),
normal=np.array([0, 0, 1]),
size=(0.3, 0.3),
n=(2, 2))
pl.uniform()
cams = create_cam_distribution(cam, pl, 0.01, plot=False)
#pl.uniform_with_distortion(mean = 0, sd = 0.01)
## OBTAIN PLANE MODEL POINTS
objectPoints_ref = pl.get_points()
#objectPoints_ref = np.load('objectPoints_test_ippe5.npy')
## CREATE AN IDEAL SET OF POINTS IN IMAGE COORDINATES (A SQUARE)
## FROM THE DESIRED IMAGE POINTS AND THE CAMERA MATRIX OBTAIN THE COORDINATES IN
## MODEL PLANE