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 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)
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))
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')
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()
#%%
# 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.')
plt.xlim(0, 1280)
plt.ylim(0, 960)
plt.show()
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_R_axisAngle(0.0, 0.0, 1.0, 0.0)
cam.set_t(0.0, 0.0, -8.0)
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()
