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 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
DataOut['epnp_rmat_error'] = [] DataOut['pnp_tvec_error'] = [] DataOut['pnp_rmat_error'] = [] homo_dlt_error_mean = [] homo_HO_error_mean = [] homo_CV_error_mean = [] ippe_tvec_error_mean = [] ippe_rmat_error_mean = [] epnp_tvec_error_mean = [] epnp_rmat_error_mean = [] pnp_tvec_error_mean = [] pnp_rmat_error_mean = [] objectPoints_historic = np.array([]).reshape(new_objectPoints.shape[0], 0) new_imagePoints = np.array(cam.project(new_objectPoints, False)) homography_iters = 1000 for i in range(50): DataOut['ObjectPoints'].append(new_objectPoints) DataOut['ImagePoints'].append(new_imagePoints) objectPoints_historic = np.hstack( [objectPoints_historic, new_objectPoints]) #PLOT IMAGE POINTS plt.sca(ax_image) ax_image.cla() cam.plot_plane(pl) ax_image.plot( new_imagePoints[0], new_imagePoints[1],
#D4pSquare.ObjectPoints.append(np.copy(objectPoints_square)) #D4pSquare.calculate_metrics() gradient_iters_max = 50 new_objectPoints = pl.get_points() #new_objectPoints = np.copy(objectPoints_square) ## GRADIENT DESCENT for i in range(gradient_iters_max): objectPoints = np.copy(new_objectPoints) gradient = gd.evaluate_gradient(gradient, objectPoints, np.array(cam.P), normalize) new_objectPoints = gd.update_points(gradient, objectPoints, limitx=0.15, limity=0.15) new_imagePoints = np.array(cam.project(new_objectPoints, False)) D5pWell.Camera.append(cam) D5pWell.ObjectPoints.append(new_objectPoints) D5pWell.calculate_metrics() print np.median(np.array(D5pIll.Homo_CV_mean) / np.array(D5pWell.Homo_CV_mean)) print np.median( np.array(D5pIll.pnp_tvec_error_mean) / np.array(D5pWell.pnp_tvec_error_mean)) print np.median(np.array(D5pIll.CondNumber) / np.array(D5pWell.CondNumber)) #print np.mean(np.array(D4pSquare.pnp_tvec_error_mean)/np.array(D4pWell.pnp_tvec_error_mean)) #print np.mean(np.array(D4pSquare.Homo_CV_mean)/np.array(D4pWell.Homo_CV_mean)) pickle.dump([D5pIll, D5pWell], open("icra_sim_illvsWell_5points.p", "wb"))
cam_positions = [] cam_orientations = [] cam_positions.append(-array([0., 0., 0.4, 1]).T) cam_positions.append(-array([0., -0.1, 0.7, 1]).T) cam_positions.append(-array([0., 0.1, 0.7, 1]).T) #%% Project screen points in image cam.set_world_position(-0.05, -0.05, -0.45) cam.rotate_x(deg2rad(+10.)) cam.rotate_y(deg2rad(-5.)) cam.rotate_z(deg2rad(-5.)) cam.set_P() cam_points1 = array(cam.project(s1.get_points())) #%% cam.set_world_position(-0.2, -0.05, -0.4) cam.rotate_y(deg2rad(-20.)) cam.set_P() cam_points2 = array(cam.project(s1.get_points())) #%% cam.set_world_position(0.2, -0.05, -0.5) cam.rotate_y(deg2rad(+40.)) cam.set_P() cam_points3 = array(cam.project(s1.get_points())) #%% plot
z = r * np.sin(np.deg2rad(angle)) cam.set_t(x, y, z) cam.set_R_mat(R_matrix_from_euler_t(0.0, 0, 0)) cam.look_at([0, 0, 0]) #Dictionary with all the important information for one pose DataSinglePose = {} DataSinglePose['Angle'] = angle DataSinglePose['Camera'] = cam.clone() DataSinglePose['Iters'] = [] DataSinglePose['ObjectPoints'] = [] DataSinglePose['ImagePoints'] = [] DataSinglePose['CondNumber'] = [] objectPoints_iter = np.copy(objectPoints_start) imagePoints_iter = np.array(cam.project(objectPoints_iter, False)) gradient = gd.create_gradient(metric='condition_number', n=n) for i in range(1000): DataSinglePose['ObjectPoints'].append(objectPoints_iter) DataSinglePose['ImagePoints'].append(imagePoints_iter) input_list = gd.extract_objectpoints_vars(objectPoints_iter) input_list.append(np.array(cam.P)) # TODO Yue: set parameter image_pts_measured as None and append it to input_list input_list.append(None) mat_cond = gd.matrix_condition_number_autograd(*input_list, normalize=False) DataSinglePose['CondNumber'].append(mat_cond)
""" 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)
s1.set_color(red) s1.curvature_radius = 1.800 s1.update() s1.rotate_x(np.deg2rad(-190.)) #s1.rotate_y(deg2rad(-15.)) #%% Project screen points in image # cam.set_world_position(-0.05, -0.05, -0.45) cam.set_t(-0.05, -0.05, -0.45, 'world') cam.rotate_x(np.deg2rad(+10.)) cam.rotate_y(np.deg2rad(-5.)) cam.rotate_z(np.deg2rad(-5.)) cam.set_P() cam_points1 = np.array(cam.project(s1.get_points())) #%% plot # plot projection plt.figure() plt.plot(cam_points1[0], cam_points1[1], '.', color='r') plt.xlim(0, 1280) plt.ylim(0, 960) plt.gca().invert_yaxis() plt.show() #%% Opencv camera calibration objp1 = np.transpose(s1.get_points_basis()[:3, :]) imgp1 = np.transpose(cam_points1[:2, :])
#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() ## we create the gradient for the point distribution normalize = False n = 0.000000001 #condition number norm
#%% # 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() #%% # create transformation r = 0.03 * np.random.rand(3) r = np.array([0.0, 0.0, 1.0]) t = np.array([0.0, 0.0, 0.1])