computer vision - Incorrect camera pose when using findEssentialMat in OpenCV

Question

I am solving the problem of camera pose estimation using findEssentialMat in OpenCV. Concretely given a set of images of some indoor scene with known pose I need to estimate the pose of some query image from the same space.

I have implemented pose estimation of query image from the best matching image using essential matrix in OpenCV and Python. I also undistort corresponding points as images of scene and query image can be captured by cameras with different intrinsics.

The problem is resultant pose of query image differ much from the ground truth and essential matrix seems to be inaccurate as matching images are very close and camera displacement should be extremely small. Here is my code:

dist_coeffs = None                                                                                                                                 
# undistort points
pts1 = cv2.undistortPoints(np.expand_dims(pts1, axis=1), cameraMatrix=K_q, distCoeffs=dist_coeffs, R=None, P=np.identity(3))                                                                                                                                                                      
pts2 = cv2.undistortPoints(np.expand_dims(pts2, axis=1), cameraMatrix=K_v, distCoeffs=dist_coeffs, R=None, P=np.identity(3))
thresh = 1.0                                                                                                                                                   
E, mask = cv2.findEssentialMat(pts1, pts2, cameraMatrix=np.identity(3), method=cv2.RANSAC, prob=F_DIST, threshold=thresh)
        
_, R_m, t_m, mask = cv2.recoverPose(E, pts1, pts2, cameraMatrix=np.identity(3))

According to official documentation, if points1 and points2 are feature points from cameras with different camera intrinsic matrix, use undistortPoints() with P = cv::NoArray() for both cameras to transform image points to normalized image coordinates, which are valid for the identity camera intrinsic matrix. When passing these coordinates, pass the identity matrix for this parameter. Here is example of essential matrix and recovered pose:

Essential matrix                                                                                                                                                        
[[ 0.23120721 -0.35422666  0.45764383]                                                                                                                                   
[ 0.27209477 -0.18381956 -0.52687146]                                                                                                                                   
[-0.32365465  0.34509926  0.04862367]]                                                                                                                                 
Recovering pose                                                                                                                                                         
t_m                                                                                                                                                                     
[[0.47250148]                                                                                                                                                            
[0.47870924]                                                                                                                                                            
[0.73998636]]                                                                                                                                                          
R_m                                                                                                                                                                     
[[ 0.84963004 -0.11140984 -0.51547711]                                                                                                                                   
[-0.10791722  0.92002069 -0.37671715]                                                                                                                                   
[ 0.5162196   0.37569906  0.76965417]]

Is my implementation correct? Do I correctly use methods undistortPoints and findEssentialMat?

Answer 1

等待大神答复