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
cam.set_K(fx=800, fy=800, cx=640 / 2., cy=480 / 2.) cam.set_width_heigth(640, 480) ## 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(