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
Hnoisy_dlt = Hnoisy_dlt / Hnoisy_dlt[2, 2]
#HO METHOD
Xo = new_objectPoints[[0, 1, 3], :]
Xi = new_imagePoints_noisy
Hnoisy_HO = hh(Xo, Xi)
#OpenCV METHOD
Xo = new_objectPoints[[0, 1, 3], :]
Xi = new_imagePoints_noisy
Hnoisy_OpenCV, _ = cv2.findHomography(Xo[:2].T.reshape(1, -1, 2),
Xi[:2].T.reshape(1, -1, 2))
## ERRORS FOR THE DLT HOMOGRAPHY
## VALIDATION OBJECT POINTS
validation_objectPoints = validation_plane.get_points()
validation_imagePoints = np.array(
cam.project(validation_objectPoints, False))
Xo = np.copy(validation_objectPoints)
Xo = np.delete(Xo, 2, axis=0)
Xi = np.copy(validation_imagePoints)
homo_dlt_error_loop.append(
ef.validation_points_error(Xi, Xo, Hnoisy_dlt))
## ERRORS FOR THE HO HOMOGRAPHY
## VALIDATION OBJECT POINTS
validation_objectPoints = validation_plane.get_points()
validation_imagePoints = np.array(
cam.project(validation_objectPoints, False))
Xo = np.copy(validation_objectPoints)
Xo = np.delete(Xo, 2, axis=0)
pl.random(n=5, 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 = gd5.create_gradient(metric='condition_number')
#gradient = gd5.create_gradient(metric='volker_metric')
#gradient = gd5.create_gradient(metric='pnorm_condition_number')
objectPoints_des = pl.get_points()
# we now replace the first 4 points with the border positions
pl.uniform()
#objectPoints_des[:,0:4] = pl.get_points()
alpha = 0.2
imagePoints_des = np.array(cam.project(objectPoints_des, False))
objectPoints_list = list()
imagePoints_list = list()
transfer_error_list = list()
condition_number_list = list()
normalized_condition_number_list = list()
new_objectPoints = objectPoints_des
for i in range(1000):
objectPoints = np.copy(new_objectPoints)
## 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
homography_iters = 1000 # TODO homography_iters changed
def heightGetCondNum(cams, accuracy_mat, theta_params, r_params):
accuracy_mat_new = np.copy(accuracy_mat)
accuracy_mat_new_rowNum = accuracy_mat_new.shape[0]
accuracy_mat_new_colNum = accuracy_mat_new.shape[1]
num_mat = np.copy(
accuracy_mat
) # each cell stores how many condition numbers in corresponding cell of accuracy_mat_new
#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]),
normal=np.array([0, 0, 1]),
size=(0.3, 0.3),
n=(4, 4))
validation_plane.uniform()