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()