def test1(objectPoints_square):
# ------------------------------Z fixed, study X Y-----------------------------------------
cams_Zfixed = []
for x in np.linspace(-0.5, 0.5, 10):
for y in np.linspace(-0.5, 0.5, 10):
cam1 = Camera()
cam1.set_K(fx=800, fy=800, cx=640 / 2., cy=480 / 2.)
cam1.set_width_heigth(640, 480)
## DEFINE A SET OF CAMERA POSES IN DIFFERENT POSITIONS BUT ALWAYS LOOKING
# TO THE CENTER OF THE PLANE MODEL
# TODO LOOK AT
cam1.set_R_axisAngle(1.0, 0.0, 0.0, np.deg2rad(180.0))
# TODO cv2.SOLVEPNP_DLS, cv2.SOLVEPNP_EPNP, cv2.SOLVEPNP_ITERATIVE
# cam1.set_t(x, -0.01, 1.31660688, frame='world')
cam1.set_t(x, y, 1.3, frame='world')
# 0.28075725, -0.23558331, 1.31660688
cams_Zfixed.append(cam1)
new_objectPoints = np.copy(objectPoints_square)
xInputs = []
yInputs = []
volumes = []
for cam in cams_Zfixed:
t = cam.get_world_position()
xInputs.append(t[0])
yInputs.append(t[1])
cov_mat = covariance_alpha_belt_r(cam, new_objectPoints)
a, b, c = ellipsoid.get_semi_axes_abc(cov_mat, 0.95)
v = ellipsoid.ellipsoid_Volume(a, b, c)
volumes.append(v)
dvm.displayCovVolume_Zfixed3D(xInputs, yInputs, volumes)
def test_f():
"""
Test f(input, K, obj_point)
:return:
"""
alpha = 0
belt = 0
r = 1
input = [0.0, 0.0, 0.0]
input[0] = alpha
input[1] = belt
input[2] = r
cam = Camera()
cam.set_K(fx=800, fy=800, cx=640 / 2., cy=480 / 2.)
cam.set_width_heigth(640, 480)
cam.set_R_mat(
Rt_matrix_from_euler_t.R_matrix_from_euler_t(0, np.deg2rad(0), 0))
cam.set_t(0., 0., 0.5, 'world')
pl = CircularPlane(origin=np.array([0., 0., 0.]),
normal=np.array([0, 0, 1]),
radius=0.15,
n=4)
x1 = round(pl.radius * np.cos(np.deg2rad(45)), 3)
y1 = round(pl.radius * np.sin(np.deg2rad(45)), 3)
objectPoints_square = np.array([[x1, -x1, -x1, x1], [y1, y1, -y1, -y1],
[0., 0., 0., 0.], [1., 1., 1., 1.]])
new_objectPoints = np.copy(objectPoints_square)
cms.f(input, cam.K, new_objectPoints)
def accuracy_func(plane_size=(0.3, 0.3),camera_intrinsic_params=(800.,800.,320.,240.), theta_params=(0.0, 180.0, 5), r_params=(0.1, 2.0, 5), z=0, y=0):
"""
Calculate the accuracy degree of given position y,z in world
y:
z:
:return 0 means the cam is out of range of marker
"""
# --------------------------------------accuracy_degree_distribution---------------------
cam = Camera()
fx = camera_intrinsic_params[0]
fy = camera_intrinsic_params[1]
cx = camera_intrinsic_params[2]
cy = camera_intrinsic_params[3]
cam.set_K(fx=fx, fy=fy, cx=cx, cy=cy)
width = cx * 2
height = cy * 2
cam.set_width_heigth(width, height)
cams,accuracy_mat = create_cam_distribution_in_YZ(cam=None, plane_size=plane_size, theta_params=theta_params, r_params=r_params,
plot=False)
accuracy_mat = np.zeros([30, 60]) # define the new acc_mat as 30 x 30 ,which means 3m x 3m area in real world and each cell is 0.1m x 0.1m
inputX, inputY, inputZ, input_ippe1_t, input_ippe1_R, input_ippe2_t, input_ippe2_R, input_pnp_t, input_pnp_R, input_transfer_error, display_mat,accuracy_mat_new = bf.heightGetCondNum(cams,accuracy_mat,theta_params, r_params)
# print accuracy_mat_new
# --------------------------------------------------------------------------------------------
degree = 5 # divide accuracy into 5 degree
r = np.sqrt(z * z + y * y)
# out of range
if r >= r_params[1] or r == 0:
return 0
else:
# according to radius and angle of cam, Calculate the index of accuracy matrix
r_begin = r_params[0]
r_end = r_params[1]
r_num = r_params[2]
r_step = (r_end - r_begin) / (r_num - 1)
# print "r_step",r_step
half_r_step = r_step / 2.0
r_num1 = math.floor(r / r_step)
r_num2 = math.floor((r - r_step * r_num1) / half_r_step)
row_index = int(r_num1 + r_num2) # row index
angle_begin = theta_params[0]
angle_end = theta_params[1]
angle_num = theta_params[2]
angle_step = (angle_end - angle_begin) / (angle_num - 1)
# print "angle_step",angle_step
half_angle_step = angle_step / 2.0
angle = np.rad2deg(np.arccos(y / r)) # deg
angle_num1 = math.floor(angle / angle_step)
angle_num2 = math.floor((angle - angle_step * angle_num1) / half_angle_step)
col_index = int(angle_num1 + angle_num2)
# print "row_index,col_index",row_index,col_index
acc_degree = accuracy_mat_new[row_index, col_index]
# print "acc_degree",acc_degree
return acc_degree
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 cam_distribution_study():
"""
cam Distribution in 3D, show the cond num distribution for each cam position
"""
number_of_points = 4
# TODO Change the radius of circular plane
pl = CircularPlane(origin=np.array([0., 0., 0.]), normal=np.array([0, 0, 1]), radius=0.15, n=4)
pl.random(n=number_of_points, r=0.01, min_sep=0.01)
plane_size = (0.3, 0.3)
cam = Camera()
cam.set_K(fx=800, fy=800, cx=640 / 2., cy=480 / 2.)
cam.set_width_heigth(640, 480)
# TODO cam distribution position PARAMETER CHANGE!!!
# cams = create_cam_distribution(cam, plane_size,
# theta_params=(0, 360, 30), phi_params=(0, 70, 10),
# r_params=(0.2, 2.0, 20), plot=False)
angle_begin = 0.0#0.0
angle_end = 180.0#180.0
angle_num = 37 #37 TODO need to set
# angle_step = (angle_end - angle_begin) / (angle_num - 1)
theta_params = (angle_begin,angle_end,angle_num)
r_begin = 0.1 # we cant set this as 0.0 !!! avoid float error!!!
r_end = 3.0
r_num = 30 #30 TODO need to set
# r_step = (r_end - r_begin) / (r_num - 1)
r_params = (r_begin,r_end,r_num)
cams,accuracy_mat = create_cam_distribution_in_YZ(cam=None, plane_size=(0.3, 0.3), theta_params=theta_params, r_params=r_params,
plot=False)
# TODO
accuracy_mat = np.zeros([30, 60])# define the new acc_mat as 30 x 30 ,which means 3m x 3m area in real world and each cel
inputX, inputY, inputZ, input_ippe1_t, input_ippe1_R, input_ippe2_t, input_ippe2_R, input_pnp_t, input_pnp_R, input_transfer_error, display_mat, accuracy_mat_new = bf.heightGetCondNum(
cams, accuracy_mat, theta_params, r_params)
# print "accuracy_mat distribution:\n",accuracy_mat_new
print "----------------Start to show:-------------------"
# print "accuracy_mat_new\n",accuracy_mat_new
# save accuracy matrix in file
print "-----accuracy_mat_new---------\n",accuracy_mat_new
smtf.saveMatToFile(accuracy_mat_new)
# plot condition number distribution
# dc.displayCondNumDistribution(display_mat)
# plot the Rt error
# dc.displayError_YZ_plane_2D(inputY, inputZ, input_ippe1_t, input_ippe1_R, input_ippe2_t, input_ippe2_R, input_pnp_t, input_pnp_R)
# dc.displayError_YZ_plane_3D(inputY, inputZ, input_ippe1_t, input_ippe1_R, input_ippe2_t, input_ippe2_R, input_pnp_t,
# input_pnp_R, input_transfer_error)
# dc.displayRTError3D_ippe12(inputY, inputZ, input_ippe1_t, input_ippe1_R, input_ippe2_t, input_ippe2_R, input_pnp_t,
# input_pnp_R, input_transfer_error)
# dc.displayRTError3D_LM(inputY, inputZ, input_ippe1_t, input_ippe1_R, input_ippe2_t, input_ippe2_R, input_pnp_t,
# input_pnp_R, input_transfer_error)
# dc.displayRTError3D_EPNP(inputY, inputZ, input_ippe1_t, input_ippe1_R, input_ippe2_t, input_ippe2_R, input_pnp_t,
# input_pnp_R, input_transfer_error)
dc.displayRTError3D_DLS(inputY, inputZ, input_ippe1_t, input_ippe1_R, input_ippe2_t, input_ippe2_R, input_pnp_t,
input_pnp_R, input_transfer_error)
print "----------------End-----------------------------"
def test2_covariance_alpha_belt_r():
"""
Test covariance_alpha_belt_r(cam, new_objectPoints)
X Y fixed, Z changed
:return:
"""
pl = CircularPlane(origin=np.array([0., 0., 0.]),
normal=np.array([0, 0, 1]),
radius=0.15,
n=4)
x1 = round(pl.radius * np.cos(np.deg2rad(45)), 3)
y1 = round(pl.radius * np.sin(np.deg2rad(45)), 3)
objectPoints_square = np.array([[x1, -x1, -x1, x1], [y1, y1, -y1, -y1],
[0., 0., 0., 0.], [1., 1., 1., 1.]])
new_objectPoints = np.copy(objectPoints_square)
# -----------------------------X Y fixed, Z changed-----------------------------
cams_XYfixed = []
for i in np.linspace(0.5, 2, 5):
cam1 = Camera()
cam1.set_K(fx=800, fy=800, cx=640 / 2., cy=480 / 2.)
cam1.set_width_heigth(640, 480)
cam1.set_R_axisAngle(1.0, 0.0, 0.0, np.deg2rad(180.0))
cam1.set_t(0, 0, i, frame='world')
# 0.28075725, -0.23558331, 1.31660688
cams_XYfixed.append(cam1)
zInputs = []
volumes = []
ellipsoid_paras = np.array([[0], [0], [0], [0], [0], [0]]) # a,b,c,x,y,z
for cam in cams_XYfixed:
cam_tem = cam.clone()
valid = cms.validCam(cam_tem, new_objectPoints)
if valid:
t = cam.get_world_position()
zInputs.append(t[2])
print "Height", t[2]
cov_mat = cms.covariance_alpha_belt_r(cam, new_objectPoints)
a, b, c = ellipsoid.get_semi_axes_abc(cov_mat, 0.95)
display_array = np.array(
[[0.0], [0.0], [0.0], [0.0], [0.0], [0.0]], dtype=float)
display_array[0:3, 0] = a, b, c
display_array[3:6, 0] = np.copy(t[0:3])
ellipsoid_paras = np.hstack((ellipsoid_paras, display_array))
v = ellipsoid.ellipsoid_Volume(a, b, c)
print "volumn", v
volumes.append(v)
dvm.displayCovVolume_XYfixed3D(zInputs, volumes)
def Z_fixed_study_square():
cams_HeightFixed = []
cam_1 = Camera()
cam_1.set_K(fx=800, fy=800, cx=640 / 2., cy=480 / 2.)
cam_1.set_width_heigth(640, 480)
cam_1.set_R_axisAngle(1.0, 0.0, 0.0, np.deg2rad(180.0))
cam_1.set_t(0.1, 0.1, 1, frame='world')
cams_HeightFixed.append(cam_1)
cam_2 = Camera()
cam_2.set_K(fx=800, fy=800, cx=640 / 2., cy=480 / 2.)
cam_2.set_width_heigth(640, 480)
cam_2.set_R_axisAngle(1.0, 0.0, 0.0, np.deg2rad(180.0))
cam_2.set_t(-0.1, 0.1, 1, frame='world')
# 0.28075725, -0.23558331, 1.31660688
cams_HeightFixed.append(cam_2)
cam_3 = Camera()
cam_3.set_K(fx=800, fy=800, cx=640 / 2., cy=480 / 2.)
cam_3.set_width_heigth(640, 480)
cam_3.set_R_axisAngle(1.0, 0.0, 0.0, np.deg2rad(180.0))
cam_3.set_t(-0.1, -0.1, 1, frame='world')
# 0.28075725, -0.23558331, 1.31660688
cams_HeightFixed.append(cam_3)
cam_4 = Camera()
cam_4.set_K(fx=800, fy=800, cx=640 / 2., cy=480 / 2.)
cam_4.set_width_heigth(640, 480)
cam_4.set_R_axisAngle(1.0, 0.0, 0.0, np.deg2rad(180.0))
cam_4.set_t(0.1, -0.1, 1, frame='world')
# 0.28075725, -0.23558331, 1.31660688
cams_HeightFixed.append(cam_4)
inputX, inputY, inputZ, input_ippe1_t, input_ippe1_R, input_ippe2_t, input_ippe2_R, input_pnp_t, input_pnp_R, input_transfer_error, display_mat = bf.heightGetCondNum(cams_HeightFixed)
def test3_covariance_alpha_belt_r():
"""
Y fixed, study X Z like a half circle above the marker
:return:
"""
pl = CircularPlane(origin=np.array([0., 0., 0.]),
normal=np.array([0, 0, 1]),
radius=0.15,
n=4)
x1 = round(pl.radius * np.cos(np.deg2rad(45)), 3)
y1 = round(pl.radius * np.sin(np.deg2rad(45)), 3)
objectPoints_square = np.array([[x1, -x1, -x1, x1], [y1, y1, -y1, -y1],
[0., 0., 0., 0.], [1., 1., 1., 1.]])
new_objectPoints = np.copy(objectPoints_square)
# ------------------------------Y fixed, study X Z like a half circle -----------------------------------------
cams_Yfixed = []
for angle in np.linspace(np.deg2rad(0), np.deg2rad(180), 20):
x = np.cos(angle)
z = np.sin(angle)
cam1 = Camera()
cam1.set_K(fx=800, fy=800, cx=640 / 2., cy=480 / 2.)
cam1.set_width_heigth(640, 480)
# TODO WE have not set R yet
# print 'cam1.R',cam1.R
cam1.set_t(x, 0, z, frame='world')
cams_Yfixed.append(cam1)
xInputs = []
zInputs = []
volumes = []
for cam in cams_Yfixed:
cam_tem = cam.clone()
valid = cms.validCam(cam_tem, new_objectPoints)
if valid:
t = cam.get_world_position()
xInputs.append(t[0])
zInputs.append(t[2])
print "x = ", t[0]
print "z = ", t[2]
cov_mat = cms.covariance_alpha_belt_r(cam, new_objectPoints)
a, b, c = ellipsoid.get_semi_axes_abc(cov_mat, 0.95)
v = ellipsoid.ellipsoid_Volume(a, b, c)
print "v", v
volumes.append(v)
dvm.displayCovVolume_Zfixed3D(xInputs, zInputs, volumes)
def test_covariance_alpha_belt_r():
"""
Test covariance_alpha_belt_r(cam, new_objectPoints)
Z fixed, study X Y-
:return:
"""
pl = CircularPlane(origin=np.array([0., 0., 0.]),
normal=np.array([0, 0, 1]),
radius=0.15,
n=4)
x1 = round(pl.radius * np.cos(np.deg2rad(45)), 3)
y1 = round(pl.radius * np.sin(np.deg2rad(45)), 3)
objectPoints_square = np.array([[x1, -x1, -x1, x1], [y1, y1, -y1, -y1],
[0., 0., 0., 0.], [1., 1., 1., 1.]])
new_objectPoints = np.copy(objectPoints_square)
# ------------------------------Z fixed, study X Y-----------------------------------------
cams_Zfixed = []
for x in np.linspace(-0.5, 0.5, 5):
for y in np.linspace(-0.5, 0.5, 5):
cam1 = Camera()
cam1.set_K(fx=800, fy=800, cx=640 / 2., cy=480 / 2.)
cam1.set_width_heigth(640, 480)
# TODO WE have not set R yet
# print 'cam1.R',cam1.R
cam1.set_t(x, y, 1.3, frame='world')
cams_Zfixed.append(cam1)
xInputs = []
yInputs = []
volumes = []
for cam in cams_Zfixed:
cam_tem = cam.clone()
valid = cms.validCam(cam_tem, new_objectPoints)
if valid:
t = cam.get_world_position()
xInputs.append(t[0])
yInputs.append(t[1])
cov_mat = cms.covariance_alpha_belt_r(cam, new_objectPoints)
a, b, c = ellipsoid.get_semi_axes_abc(cov_mat, 0.95)
v = ellipsoid.ellipsoid_Volume(a, b, c)
volumes.append(v)
dvm.displayCovVolume_Zfixed3D(xInputs, yInputs, volumes)
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
number_of_points = 4
if number_of_points == 4:
import gdescent.hpoints_gradient as gd
elif number_of_points == 5:
import gdescent.hpoints_gradient5 as gd
elif number_of_points == 6:
import gdescent.hpoints_gradient6 as gd
## Define a Display plane with random initial points
pl = CircularPlane()
pl.random(n=number_of_points, r=0.01, min_sep=0.01)
## 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)
## DEFINE CAMERA POSE LOOKING STRAIGTH DOWN INTO THE PLANE MODEL
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')
#cam.set_R_axisAngle(1.0, 0.0, 0.0, np.deg2rad(140.0))
#cam.set_t(0.0,-1,1.0, frame='world')
#
r = 0.8
angle = 30
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))
@author: lracuna """ 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.))
#define the plots
#one Figure for image an object points
fig1 = plt.figure('Image and Object points')
ax_image = fig1.add_subplot(211)
ax_object = fig1.add_subplot(212)
## we create the gradient for the point distribution
normalize = False
n = 0.00000001 #condition number norm 4 points
n = 0.000001 #condition number norm 4 points
#n = 0.0000001 #condition number norm 5 points
gradient = gd.create_gradient(metric='condition_number', n=n)
## 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)
#Define a set of angles and a distance
r = 0.8
angles = np.arange(90, 91, 20)
#START OF THE MAIN LOOP
#List with the results for each camera pose
DataSim = []
for angle in angles:
#Move the camera to the next pose
x = r * np.cos(np.deg2rad(angle))
y = 0
"""
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)
List of objects of the type Plane
Returns
-------
None
"""
# mlab.figure(figure=None, bgcolor=(0.1,0.5,0.5), fgcolor=None,
# engine=None, size=(400, 350))
axis_scale = 0.05
for cam in cams:
plot3D_cam(cam, axis_scale)
for fiducial_space in planes:
# Plot plane points in 3D
plotPoints3D(fiducial_space)
display_figure()
cam = Camera()
cam.set_K(fx=100, fy=100, cx=640, cy=480)
cam.set_width_heigth(1280, 960)
""" Initial camera pose looking straight down into the plane model """
cam.set_R_axisAngle(1.0, 0.0, 0.0, np.deg2rad(180))
cam.set_t(0.0, 0.0, 1.5, frame='world')
""" Plane for the control points """
sph = Sphere(2)
sph.set_color((1, 1, 0))
sph.random_points(3000, 0.3, 0.0001)
""" Plot camera axis and plane """
cams = [cam]
planes = [sph]
plot3D(cams, planes)
from solve_ippe import pose_ippe_both, pose_ippe_best
from solve_pnp import pose_pnp
import cv2
number_of_points = 4
if number_of_points == 4:
import gdescent.hpoints_gradient as gd
elif number_of_points == 5:
import gdescent.hpoints_gradient5 as gd
elif number_of_points == 6:
import gdescent.hpoints_gradient6 as gd
## CREATE A SIMULATED CAMERA
cam = Camera()
cam.set_K(fx=800, fy=800, cx=640, cy=480)
cam.set_width_heigth(960, 960)
## DEFINE CAMERA POSE LOOKING STRAIGTH DOWN INTO THE PLANE MODEL
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')
#cam.set_R_axisAngle(1.0, 0.0, 0.0, np.deg2rad(140.0))
#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))
class OptimalPointsSim(object):
""" Class that defines and optimization to obtain optimal control points
configurations for homography and plannar pose estimation. """
def __init__(self):
""" Definition of a simulated camera """
self.cam = Camera()
self.cam.set_K(fx=100, fy=100, cx=640, cy=480)
self.cam.set_width_heigth(1280, 960)
""" Initial camera pose looking stratight down into the plane model """
self.cam.set_R_axisAngle(1.0, 0.0, 0.0, np.deg2rad(180))
self.cam.set_t(0.0, 0.0, 1.5, frame='world')
""" Plane for the control points """
self.sph = Sphere(radius=0.5)
self.sph.random_points(p=6, r=0.5, min_dist=0.001)
def run(self):
self.objectPoints = self.sph.get_sphere_points()
self.init_params = flatten_points(self.objectPoints, type='object')
self.objective1 = lambda params: matrix_condition_number_autograd(
params, self.cam.P, normalize=False)
self.objective2 = lambda params, iter: matrix_condition_number_autograd(
params, self.cam.P, normalize=True)
print("Optimizing condition number...")
objective_grad = grad(self.objective2)
self.optimized_params = adam(objective_grad,
self.init_params,
step_size=0.001,
num_iters=200,
callback=self.plot_points)
def plot_points(self, params, iter, gradient):
phisim = np.linspace((-math.pi) / 2., (math.pi / 2.))
thetasim = np.linspace(0, 2 * np.pi)
print params
pointscoord = np.full((3, 6), 0.0)
for i in range(6):
pointscoord[0, i] = params[i]
pointscoord[1, i] = params[i + 1]
pointscoord[2, i] = params[i + 2]
x = np.outer(np.sin(thetasim), np.cos(phisim))
y = np.outer(np.sin(thetasim), np.sin(phisim))
z = np.outer(np.cos(thetasim), np.ones_like(phisim))
fig, ax = plt.subplots(subplot_kw={'projection': '3d'})
ax.plot_wireframe(sph.radius * x,
sph.radius * y,
sph.radius * z,
color='g')
ax.scatter(pointscoord[:3, 0],
pointscoord[:3, 1],
pointscoord[:3, 2],
c='r')
ax.scatter(pointscoord[:3, 3],
pointscoord[:3, 4],
pointscoord[:3, 5],
c='r')
plt.show()
from vision.camera import Camera
from vision.rt_matrix import *
import numpy as np
from matplotlib import pyplot as plt
#%%
# load points
points = np.loadtxt('house.p3d').T
points = np.vstack((points, np.ones(points.shape[1])))
#%%
# 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.')
