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
ImageNoise = 4 DataOut['ImageNoise'] = ImageNoise # WE CREATE NOISY IMAGE POINTS (BASED ON THE TRUE VALUES) AND CALCULATE # THE ERRORS WE THEN OBTAIN AN AVERAGE FOR EACH ONE for j in range(homography_iters): new_imagePoints_noisy = cam.addnoise_imagePoints(new_imagePoints, mean=0, sd=ImageNoise) #Calculate the pose using IPPE (solution with least repro error) normalizedimagePoints = cam.get_normalized_pixel_coordinates( new_imagePoints_noisy) ippe_tvec1, ippe_rmat1, ippe_tvec2, ippe_rmat2 = pose_ippe_both( new_objectPoints, normalizedimagePoints, debug=False) ippeCam1 = cam.clone_withPose(ippe_tvec1, ippe_rmat1) ippeCam2 = cam.clone_withPose(ippe_tvec2, ippe_rmat2) #Calculate the pose using solvepnp EPNP debug = False epnp_tvec, epnp_rmat = pose_pnp(new_objectPoints, new_imagePoints_noisy, cam.K, debug, cv2.SOLVEPNP_EPNP, False) epnpCam = cam.clone_withPose(epnp_tvec, epnp_rmat) #Calculate the pose using solvepnp ITERATIVE pnp_tvec, pnp_rmat = pose_pnp(new_objectPoints, new_imagePoints_noisy, cam.K, debug, cv2.SOLVEPNP_ITERATIVE, False) pnpCam = cam.clone_withPose(pnp_tvec, pnp_rmat)
new_objectPoints = objectPoints_des new_imagePoints = np.array(cam.project(new_objectPoints, False)) homography_iters = 1 for i in range(1000): # Xo = np.copy(new_objectPoints[[0,1,3],:]) #without the z coordinate (plane) # Xi = np.copy(new_imagePoints) # Aideal = ef.calculate_A_matrix(Xo, Xi) objectPoints_list.append(new_objectPoints) objectPoints_historic = np.hstack([objectPoints_historic,new_objectPoints]) imagePoints_list.append(new_imagePoints) #Calculate the pose using solvepnp ITERATIVE new_imagePoints_noisy = cam.addnoise_imagePoints(new_imagePoints, mean = 0, sd = 2) pnp_tvec, pnp_rmat = pose_pnp(new_objectPoints, new_imagePoints_noisy, cam.K, False, cv2.SOLVEPNP_ITERATIVE,False) pnpCam = cam.clone_withPose(pnp_tvec, pnp_rmat) x1,y1,x2,y2,x3,y3,x4,y4 = gd.extract_objectpoints_vars(new_objectPoints) repro = gd.repro_error_autograd(x1,y1,x2,y2,x3,y3,x4,y4,pnpCam.P, new_imagePoints_noisy) print "--------------------------------" print "Repro Error: ", repro print "--------------------------------" # # input_list = gd.extract_objectpoints_vars(new_objectPoints) # input_list.append(np.array(cam.P)) # # mat_cond = gd.matrix_condition_number_autograd(*input_list, normalize = False) # # input_list = gd.extract_objectpoints_vars(new_objectPoints) # input_list.append(np.array(cam.P)) # mat_cond_normalized = gd.matrix_condition_number_autograd(*input_list, normalize = True)