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 getCondNum_camPoseInRealWord(x_w, y_w, grid_reso, width, height):
"""
Compute the condition number of camera position in real world coordinate
:param x_w: camera potion in real world coordinate
:param y_w: camera potion in real world coordinate
:param width: gird 60
:param height: grid 30
:return:
"""
# width = int(grid_width/ grid_reso)
# height = int(grid_height/ grid_reso)
T_WM = getMarkerTransformationMatrix(width, height, grid_reso)
pos_world_homo = np.array([x_w, y_w, 0, 1])
pos_marker = np.dot(T_WM, pos_world_homo)
## 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)
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))
cam.look_at([0, 0, 0])
radius = np.sqrt(pos_marker[0]**2 + pos_marker[1]**2 + pos_marker[2]**2)
angle = np.rad2deg(np.arccos(pos_marker[1] / radius))
cam.set_radius(radius)
cam.set_angle(angle)
objectPoints = np.copy(new_objectPoints)
imagePoints = np.array(cam.project(objectPoints, False))
condNum = np.inf # undetected region set as 0
if ((imagePoints[0, :] < cam.img_width) & (imagePoints[0, :] > 0) &
(imagePoints[1, :] < cam.img_height) & (imagePoints[1, :] > 0)).all():
input_list = gd.extract_objectpoints_vars(objectPoints)
input_list.append(np.array(cam.K))
input_list.append(np.array(cam.R))
input_list.append(cam.t[0, 3])
input_list.append(cam.t[1, 3])
input_list.append(cam.t[2, 3])
input_list.append(cam.radius)
# TODO normalize points!!! set normalize as default True
condNum = gd.matrix_condition_number_autograd(*input_list,
normalize=True)
return condNum
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
DataOut['epnp_rmat_error'] = []
DataOut['pnp_tvec_error'] = []
DataOut['pnp_rmat_error'] = []
homo_dlt_error_mean = []
homo_HO_error_mean = []
homo_CV_error_mean = []
ippe_tvec_error_mean = []
ippe_rmat_error_mean = []
epnp_tvec_error_mean = []
epnp_rmat_error_mean = []
pnp_tvec_error_mean = []
pnp_rmat_error_mean = []
objectPoints_historic = np.array([]).reshape(new_objectPoints.shape[0], 0)
new_imagePoints = np.array(cam.project(new_objectPoints, False))
homography_iters = 1000
for i in range(50):
DataOut['ObjectPoints'].append(new_objectPoints)
DataOut['ImagePoints'].append(new_imagePoints)
objectPoints_historic = np.hstack(
[objectPoints_historic, new_objectPoints])
#PLOT IMAGE POINTS
plt.sca(ax_image)
ax_image.cla()
cam.plot_plane(pl)
ax_image.plot(
new_imagePoints[0],
new_imagePoints[1],
#D4pSquare.ObjectPoints.append(np.copy(objectPoints_square))
#D4pSquare.calculate_metrics()
gradient_iters_max = 50
new_objectPoints = pl.get_points()
#new_objectPoints = np.copy(objectPoints_square)
## GRADIENT DESCENT
for i in range(gradient_iters_max):
objectPoints = np.copy(new_objectPoints)
gradient = gd.evaluate_gradient(gradient, objectPoints,
np.array(cam.P), normalize)
new_objectPoints = gd.update_points(gradient,
objectPoints,
limitx=0.15,
limity=0.15)
new_imagePoints = np.array(cam.project(new_objectPoints, False))
D5pWell.Camera.append(cam)
D5pWell.ObjectPoints.append(new_objectPoints)
D5pWell.calculate_metrics()
print np.median(np.array(D5pIll.Homo_CV_mean) / np.array(D5pWell.Homo_CV_mean))
print np.median(
np.array(D5pIll.pnp_tvec_error_mean) /
np.array(D5pWell.pnp_tvec_error_mean))
print np.median(np.array(D5pIll.CondNumber) / np.array(D5pWell.CondNumber))
#print np.mean(np.array(D4pSquare.pnp_tvec_error_mean)/np.array(D4pWell.pnp_tvec_error_mean))
#print np.mean(np.array(D4pSquare.Homo_CV_mean)/np.array(D4pWell.Homo_CV_mean))
pickle.dump([D5pIll, D5pWell], open("icra_sim_illvsWell_5points.p", "wb"))
cam_positions = [] cam_orientations = [] cam_positions.append(-array([0., 0., 0.4, 1]).T) cam_positions.append(-array([0., -0.1, 0.7, 1]).T) cam_positions.append(-array([0., 0.1, 0.7, 1]).T) #%% Project screen points in image cam.set_world_position(-0.05, -0.05, -0.45) cam.rotate_x(deg2rad(+10.)) cam.rotate_y(deg2rad(-5.)) cam.rotate_z(deg2rad(-5.)) cam.set_P() cam_points1 = array(cam.project(s1.get_points())) #%% cam.set_world_position(-0.2, -0.05, -0.4) cam.rotate_y(deg2rad(-20.)) cam.set_P() cam_points2 = array(cam.project(s1.get_points())) #%% cam.set_world_position(0.2, -0.05, -0.5) cam.rotate_y(deg2rad(+40.)) cam.set_P() cam_points3 = array(cam.project(s1.get_points())) #%% plot
z = r * np.sin(np.deg2rad(angle))
cam.set_t(x, y, z)
cam.set_R_mat(R_matrix_from_euler_t(0.0, 0, 0))
cam.look_at([0, 0, 0])
#Dictionary with all the important information for one pose
DataSinglePose = {}
DataSinglePose['Angle'] = angle
DataSinglePose['Camera'] = cam.clone()
DataSinglePose['Iters'] = []
DataSinglePose['ObjectPoints'] = []
DataSinglePose['ImagePoints'] = []
DataSinglePose['CondNumber'] = []
objectPoints_iter = np.copy(objectPoints_start)
imagePoints_iter = np.array(cam.project(objectPoints_iter, False))
gradient = gd.create_gradient(metric='condition_number', n=n)
for i in range(1000):
DataSinglePose['ObjectPoints'].append(objectPoints_iter)
DataSinglePose['ImagePoints'].append(imagePoints_iter)
input_list = gd.extract_objectpoints_vars(objectPoints_iter)
input_list.append(np.array(cam.P))
# TODO Yue: set parameter image_pts_measured as None and append it to input_list
input_list.append(None)
mat_cond = gd.matrix_condition_number_autograd(*input_list,
normalize=False)
DataSinglePose['CondNumber'].append(mat_cond)
"""
H = H_DLTwithfor(worldpoints, imagepoints, numberpoints, True)
U, s, V = np.linalg.svd(H, full_matrices=False)
print s
greatest_singular_value = s[0]
smallest_singular_value = s[11]
# print greatest_singular_value, smallest_singular_value
return greatest_singular_value/smallest_singular_value
def get_conditioning_number(A):
return np.linalg.cond(A)
# ---------- test -------------------------------------------
cam = Camera()
sph = Sphere()
cam.set_K(fx=800., fy=800., cx=640., cy=480.)
cam.set_width_heigth(1280, 960)
imagePoints = np.full((2, 6), 0.0)
cam.set_R_axisAngle(1.0, 0.0, 0.0, np.deg2rad(180.0))
cam.set_t(0.0, -0.0, 0.5, frame='world')
worldpoints = sph.random_points(6, 0.3)
imagePoints = cam.project(worldpoints, False)
H = DLT3D(cam,worldpoints, imagePoints, True)
DLTimage = DLTproject(H, worldpoints)
s1.set_color(red) s1.curvature_radius = 1.800 s1.update() s1.rotate_x(np.deg2rad(-190.)) #s1.rotate_y(deg2rad(-15.)) #%% Project screen points in image # cam.set_world_position(-0.05, -0.05, -0.45) cam.set_t(-0.05, -0.05, -0.45, 'world') cam.rotate_x(np.deg2rad(+10.)) cam.rotate_y(np.deg2rad(-5.)) cam.rotate_z(np.deg2rad(-5.)) cam.set_P() cam_points1 = np.array(cam.project(s1.get_points())) #%% plot # plot projection plt.figure() plt.plot(cam_points1[0], cam_points1[1], '.', color='r') plt.xlim(0, 1280) plt.ylim(0, 960) plt.gca().invert_yaxis() plt.show() #%% Opencv camera calibration objp1 = np.transpose(s1.get_points_basis()[:3, :]) imgp1 = np.transpose(cam_points1[:2, :])
#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()
## we create the gradient for the point distribution
normalize = False
n = 0.000000001 #condition number norm
#%% # setup camera #P = hstack((eye(3),array([[0],[0],[-10]]))) cam = Camera() ## Test matrix functions cam.set_K(1460, 1460, 608, 480) cam.set_width_heigth(1280, 960) #TODO Yue # cam.set_R(0.0, 0.0, 1.0, 0.0) cam.set_t(0.0, 0.0, -8.0) cam.set_R_axisAngle(1.0, 0.0, 0.0, np.deg2rad(140.0)) #TODO Yue cam.set_P() print(cam.factor()) #%% x = np.array(cam.project(points)) #%% # plot projection plt.figure() plt.plot(x[0], x[1], 'k.') plt.xlim(0, 1280) plt.ylim(0, 960) plt.show() #%% # create transformation r = 0.03 * np.random.rand(3) r = np.array([0.0, 0.0, 1.0]) t = np.array([0.0, 0.0, 0.1])
