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
Hnoisy_dlt = Hnoisy_dlt / Hnoisy_dlt[2, 2] #HO METHOD Xo = new_objectPoints[[0, 1, 3], :] Xi = new_imagePoints_noisy Hnoisy_HO = hh(Xo, Xi) #OpenCV METHOD Xo = new_objectPoints[[0, 1, 3], :] Xi = new_imagePoints_noisy Hnoisy_OpenCV, _ = cv2.findHomography(Xo[:2].T.reshape(1, -1, 2), Xi[:2].T.reshape(1, -1, 2)) ## ERRORS FOR THE DLT HOMOGRAPHY ## VALIDATION OBJECT POINTS validation_objectPoints = validation_plane.get_points() validation_imagePoints = np.array( cam.project(validation_objectPoints, False)) Xo = np.copy(validation_objectPoints) Xo = np.delete(Xo, 2, axis=0) Xi = np.copy(validation_imagePoints) homo_dlt_error_loop.append( ef.validation_points_error(Xi, Xo, Hnoisy_dlt)) ## ERRORS FOR THE HO HOMOGRAPHY ## VALIDATION OBJECT POINTS validation_objectPoints = validation_plane.get_points() validation_imagePoints = np.array( cam.project(validation_objectPoints, False)) Xo = np.copy(validation_objectPoints) Xo = np.delete(Xo, 2, axis=0)
pl.random(n=5, r=0.001, min_sep=0.001) ## 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.5, 0.5), n=(4, 4)) validation_plane.uniform() ## we create the gradient for the point distribution gradient = gd5.create_gradient(metric='condition_number') #gradient = gd5.create_gradient(metric='volker_metric') #gradient = gd5.create_gradient(metric='pnorm_condition_number') objectPoints_des = pl.get_points() # we now replace the first 4 points with the border positions pl.uniform() #objectPoints_des[:,0:4] = pl.get_points() alpha = 0.2 imagePoints_des = np.array(cam.project(objectPoints_des, False)) objectPoints_list = list() imagePoints_list = list() transfer_error_list = list() condition_number_list = list() normalized_condition_number_list = list() new_objectPoints = objectPoints_des for i in range(1000): objectPoints = np.copy(new_objectPoints)
## 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( accuracy_mat ) # each cell stores how many condition numbers in corresponding cell of accuracy_mat_new
#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') ## 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()