I used the below code to verify if findEssentialMat actually performs correctly but I got wrong result. Ideally, with same pts1 and pts2, the returned E should be a near zero matrix. Also the downstream recovered t_new should be close to zero too. However, I got wrong result as follow
import numpy as np
import cv2
# Define the intrinsic camera matrix K (example values)
K_identity = np.eye(3, dtype=np.float64) # Using an identity matrix for debugging
# Create two sets of identical points to simulate perfect matches
pts1 = np.float32([[100, 100], [0, 200], [350, 300], [230, 400], [100, 500], [700, 200]]).reshape(-1, 1, 2)
pts2 = np.float32([[100, 100], [0, 200], [350, 300], [230, 400], [100, 500], [700, 200]]).reshape(-1, 1, 2)
# pts2 = pts1 + 0
print(pts1.shape)
# Compute the essential matrix using the perfectly matched points with the identity intrinsic matrix
E, mask = cv2.findEssentialMat(pts1, pts2, K_identity, method=cv2.RANSAC, prob=0.999, threshold=1)
# Print the essential matrix
print(f"Essential Matrix E with identity K:\n{E}, {E.shape}")
# Check the epipolar constraint for each pair of points
print("\nEpipolar Constraints (should be close to zero):")
for pt1, pt2 in zip(pts1, pts2):
pt1_h = np.append(pt1.flatten(), 1).reshape(3, 1)
pt2_h = np.append(pt2.flatten(), 1).reshape(1, 3)
epipolar_constraint = pt2_h @ E @ pt1_h
print(f"Epipolar constraint for point pair {pt1.flatten()} and {pt2.flatten()}: {epipolar_constraint[0, 0]}")
# Perform SVD on the essential matrix to ensure it has the correct properties
U, S, Vt = np.linalg.svd(E)
print("\nSingular Values of E (should be close to [1, 1, 0]):")
print(S)
# Recover the pose
_, R_new, t_new, mask = cv2.recoverPose(E, pts1, pts2, K_identity)
# Print the recovered rotation and translation
print(f"\nRecovered Rotation Matrix R_new with identity K:\n{R_new}")
print(f"Recovered Translation Vector t_new with identity K (before normalization):\n{t_new}")
# Check if the translation vector needs normalization
t_new_normalized = t_new / np.linalg.norm(t_new)
print(f"Normalized Translation Vector t_new with identity K:\n{t_new_normalized}")
# Check if the camera has moved significantly
if np.allclose(R_new, np.eye(3), atol=1e-2) and np.allclose(t_new, np.zeros((3, 1)), atol=1e-2):
print("Camera has not moved significantly.")
else:
print("Camera has moved.")
# Check if the camera has moved significantly using the normalized translation vector
if np.allclose(R_new, np.eye(3), atol=1e-2) and np.allclose(t_new_normalized, np.zeros((3, 1)), atol=1e-2):
print("Camera has not moved significantly (using normalized translation vector).")
else:
print("Camera has moved (using normalized translation vector).")
Results：
(6, 1, 2)
Essential Matrix E with identity K:
[[ 1.52435593e-17 -4.08248290e-01 -4.08248290e-01]
[ 4.08248290e-01 8.95159564e-18 -4.08248290e-01]
[ 4.08248290e-01 4.08248290e-01 -2.52770321e-17]], (3, 3)
Epipolar Constraints (should be close to zero):
Epipolar constraint for point pair [100. 100.] and [100. 100.]: 3.836930773104541e-13
Epipolar constraint for point pair [ 0. 200.] and [ 0. 200.]: 3.552713678800501e-13
Epipolar constraint for point pair [350. 300.] and [350. 300.]: 4.433786671143025e-12
Epipolar constraint for point pair [230. 400.] and [230. 400.]: 2.6716406864579767e-12
Epipolar constraint for point pair [100. 500.] and [100. 500.]: 5.9401372709544376e-12
Epipolar constraint for point pair [700. 200.] and [700. 200.]: 1.028865881380625e-11
Singular Values of E (should be close to [1, 1, 0]):
[7.07106781e-01 7.07106781e-01 7.70079232e-18]
Recovered Rotation Matrix R_new with identity K:
[[-0.33333333 -0.66666667 0.66666667]
[-0.66666667 -0.33333333 -0.66666667]
[ 0.66666667 -0.66666667 -0.33333333]]
Recovered Translation Vector t_new with identity K (before normalization):
[[ 0.57735027]
[-0.57735027]
[ 0.57735027]]
Normalized Translation Vector t_new with identity K:
[[ 0.57735027]
[-0.57735027]
[ 0.57735027]]
Camera has moved.
Camera has moved (using normalized translation vector).
idk what’s wrong with this. can someone help?