OpenCV对来自2张图像的点进行三角剖分，以估计第三张上的姿势

编程问答 2023-02-13

问题描述

我想估计当前图像相对于数据库中匹配图像的姿态。这些图像来自具有适当测距法的运动机器人，因此我可以拍摄两个连续的图像，这些图像都与数据库中的图像相似，并且可以很好地估计这两个图像之间的相对姿势。我想使用该信息来使用triangulatePoints估计两个当前图像中匹配的关键点之间的3D关系，然后使用solvePnPRansac使用也与之匹配的关键点来估计数据库中图像相对于第一当前图像的姿态

如果已尝试使用Python在OpenCV中实现此功能，如下所示。当前，我不使用数据库映像，只是在确认是否可以获取我强制执行的里程表，但是不幸的是，当前输出是垃圾（按e + 10的顺序转换）。

import numpy as np
import cv2

# Fisheye camera and distortion matrices
K=np.array([[455.5000196386718,0.0,482.65324003911945],[0.0,340.6409393462825,254.5063795692748],1.0]])
D=np.array([[-0.018682808343432777],[-0.044315351694893736],[0.047678551616171246],[-0.018283908577445218]])

orb = cv2.ORB_create(nfeatures=1000)
bf = cv2.BFMatcher(cv2.norM_HAMMING,crossCheck=False)

img0 = cv2.imread("0cm.png")
img2 = cv2.imread("2cm.png")

# Find keypoints and match them up
kp0,des0 = orb.detectAndCompute(img0,None)
kp2,des2 = orb.detectAndCompute(img2,None)
matches = bf.knnMatch(des0,des2,k=2)

# Find good matches using the ratio test
ratio_thresh = 0.8
good_matches = []
for m,n in matches:
    if m.distance < ratio_thresh * n.distance:
        good_matches.append(m)

# Convert from keypoints to points
pts0 = np.float32([kp0[m.queryIdx].pt for m in good_matches]).reshape(-1,1,2)
pts2 = np.float32([kp2[m.trainIdx].pt for m in good_matches]).reshape(-1,2)

# Remove the fisheye distortion from the points
pts0 = cv2.fisheye.undistortPoints(pts0,K,D,P=K)
pts2 = cv2.fisheye.undistortPoints(pts2,P=K)

# Keep only the points that make geometric sense
# Todo: find a more efficient way to apply the mask
E,mask = cv2.findEssentialMat(pts0,pts2,cv2.RANSAC,0.999,None)
_,R,t,mask = cv2.recoverPose(E,pts0,cameraMatrix=K,mask=mask)
pts0_m = []
pts2_m = []
for i in range(len(mask)):
    if mask[i] == 1:
        pts0_m.append(pts0[i])
        pts2_m.append(pts2[i])
pts0 = np.array(pts0_m)
pts2 = np.array(pts2_m)

# Setup the projection matrices
R = np.eye(3)
t0 = np.array([[0],[0],[0]])
t2 = np.array([[0],[2]])
P0 = np.dot(K,np.concatenate((R,t0),axis=1))
P2 = np.dot(K,t2),axis=1))

# Find the keypoint world homogeneous coordinates assuming img0 is the world origin
X = cv2.triangulatePoints(P0,P2,pts2)

# Convert from homogeneous cooridinates,Todo: find a more efficient way
objPts = []
for pt in X.T:
    objPts.append([pt[0]/pt[3],pt[1]/pt[3],pt[2]/pt[3]])
objPts = np.array(objPts)

# Find the pose of the second frame
_,rvec,tvec,inliers = cv2.solvePnPRansac(objPts,None)
print(rvec)
print(tvec)

我的代码或方法（或两者）是否有问题？

编辑：

我用6厘米远而不是2厘米远的图像对其进行了测试，然后看来效果很好。我猜想仅在向前方向上进行小的转换会导致某种形式的数值不稳定。尽管根据我用来构造投影矩阵的单位，结果会有细微的差别（米为0.06，厘米为6），但是我不知道我应该实际使用什么单位来获得最准确的结果，如下所示：似乎无关紧要。尽管它可能与照相机矩阵有关，但是我使用https://medium.com/@kennethjiang/calibrate-fisheye-lens-using-opencv-333b05afa0b0上的指南获得了我的信息，并且缩放校准代码中棋盘格的尺寸对结果矩阵没有影响，所以现在我不知道。

解决方法

暂无找到可以解决该程序问题的有效方法，小编努力寻找整理中！

如果你已经找到好的解决方法，欢迎将解决方案带上本链接一起发送给小编。

小编邮箱:dio#foxmail.com (将#修改为@）

opencvopencvopencv-pythonpythonrobotics