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
#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)
#Calculate errors
ippe_tvec_error1, ippe_rmat_error1 = ef.calc_estimated_pose_error(
cam.get_tvec(), cam.R, ippeCam1.get_tvec(), ippe_rmat1)
ippe_tvec_error_loop.append(ippe_tvec_error1)
ippe_rmat_error_loop.append(ippe_rmat_error1)
epnp_tvec_error, epnp_rmat_error = ef.calc_estimated_pose_error(
cam.get_tvec(), cam.R, epnpCam.get_tvec(), epnp_rmat)
epnp_tvec_error_loop.append(epnp_tvec_error)
epnp_rmat_error_loop.append(epnp_rmat_error)
pnp_tvec_error, pnp_rmat_error = ef.calc_estimated_pose_error(
cam.get_tvec(), cam.R, pnpCam.get_tvec(), pnp_rmat)
pnp_tvec_error_loop.append(pnp_tvec_error)
pnp_rmat_error_loop.append(pnp_rmat_error)
#Homography Estimation from noisy image points
# 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)
#
# #Calculate errors
# ippe_tvec_error1, ippe_rmat_error1 = ef.calc_estimated_pose_error(cam.get_tvec(), cam.R, ippeCam1.get_tvec(), ippe_rmat1)
# ippe_tvec_error2, ippe_rmat_error2 = ef.calc_estimated_pose_error(cam.get_tvec(), cam.R, ippeCam2.get_tvec(), ippe_rmat2)
# ippe_tvec_error_loop1.append(ippe_tvec_error1)
# ippe_rmat_error_loop1.append(ippe_rmat_error1)
# ippe_tvec_error_loop2.append(ippe_tvec_error2)
# ippe_rmat_error_loop2.append(ippe_rmat_error2)
#
# epnp_tvec_error, epnp_rmat_error = ef.calc_estimated_pose_error(cam.get_tvec(), cam.R, epnpCam.get_tvec(), epnp_rmat)
# epnp_tvec_error_loop.append(epnp_tvec_error)
# epnp_rmat_error_loop.append(epnp_rmat_error)
#
pnp_tvec_error, pnp_rmat_error = ef.calc_estimated_pose_error(cam.get_tvec(), cam.R, pnpCam.get_tvec(), pnp_rmat)
print pnp_tvec_error
#pnp_tvec_error_loop.append(pnp_tvec_error)
#pnp_rmat_error_loop.append(pnp_rmat_error)
#
#
#
#
#
# #Homography Estimation from noisy image points
# Xo = new_objectPoints[[0,1,3],:]
# Xi = new_imagePoints_noisy
# #Hnoisy,A_t_ref,H_t = homo2d.homography2d(Xo,Xi)
# #Hnoisy = Hnoisy/Hnoisy[2,2]
# Hnoisy = hh(Xo,Xi)
#
# pnpCam = cam.clone_withPose(pnp_tvec, pnp_rmat)
#
# #Calculate errors
# ippe_tvec_error1, ippe_rmat_error1 = ef.calc_estimated_pose_error(cam.get_tvec(), cam.R, ippeCam1.get_tvec(), ippe_rmat1)
# ippe_tvec_error2, ippe_rmat_error2 = ef.calc_estimated_pose_error(cam.get_tvec(), cam.R, ippeCam2.get_tvec(), ippe_rmat2)
# ippe_tvec_error_loop1.append(ippe_tvec_error1)
# ippe_rmat_error_loop1.append(ippe_rmat_error1)
# ippe_tvec_error_loop2.append(ippe_tvec_error2)
# ippe_rmat_error_loop2.append(ippe_rmat_error2)
#
# epnp_tvec_error, epnp_rmat_error = ef.calc_estimated_pose_error(cam.get_tvec(), cam.R, epnpCam.get_tvec(), epnp_rmat)
# epnp_tvec_error_loop.append(epnp_tvec_error)
# epnp_rmat_error_loop.append(epnp_rmat_error)
#
pnp_tvec_error, pnp_rmat_error = ef.calc_estimated_pose_error(
cam.get_tvec(), cam.R, pnpCam.get_tvec(), pnp_rmat)
print pnp_tvec_error
#pnp_tvec_error_loop.append(pnp_tvec_error)
#pnp_rmat_error_loop.append(pnp_rmat_error)
#
#
#
#
#
# #Homography Estimation from noisy image points
# Xo = new_objectPoints[[0,1,3],:]
# Xi = new_imagePoints_noisy
# #Hnoisy,A_t_ref,H_t = homo2d.homography2d(Xo,Xi)
# #Hnoisy = Hnoisy/Hnoisy[2,2]
# Hnoisy = hh(Xo,Xi)
#