使用 SolvePNP 进行姿态估计

Question

我试图解决几个俯瞰阿拉斯加冰川的延时相机的姿势。 到目前为止，所有估计的姿势都非常不准确。 我用于真实世界坐标的单位是 UTM（东/北）和高程。 我相信我的问题在于单位错误，或者世界坐标与图像坐标不匹配。

这是我对焦距和地面控制的了解：

焦距：5740.0 像素

世界：[3.93610609e+05 6.695578​​33e+06 7.82287000e+02] 图片：[479.2448.]

世界：[3.93506713e+05 6.69585564e+06 9.61337000e+02] 图片：[164.1398.]

世界：[3.94569509e+05 6.69555068e+06 6.21075000e+02] 图片：[2812. 3853.]

世界：[3.97774e+05 6.69986e+06 1.64200e+03] 图片：[6310。 1398.]

self.tvec = (393506.713,6695855.641,961.337) self.rvec = (np.radians(0),np.radians(0),np.radians(15))

def estimatePose(self):
    print("Estimating Pose for ", str(self.instance),"\n")
    _,self.rvec,self.tvec,_ = cv2.solvePnPRansac(self.worldGCP,self.imgGCP,self.cameraMatrix,iterationsCount=10000,distCoeffs=None,rvec=self.rvec,tvec=self.tvec,useExtrinsicGuess=1)
    self.R = np.zeros((3,3))
    cv2.Rodrigues(self.rvec,self.R)
    angle = np.degrees(self.rotationMatrixToEulerAngles(self.R))
    self.R = np.append(self.R,self.tvec,1)
    self.world2img = self.cameraMatrix@self.R

def rotationMatrixToEulerAngles(self, R) :

    sy = math.sqrt(R[0,0] * R[0,0] +  R[1,0] * R[1,0])
    singular = sy < 1e-6
    if  not singular :
        x = math.atan2(R[2,1] , R[2,2])
        y = math.atan2(-R[2,0], sy)
        z = math.atan2(R[1,0], R[0,0])
    else :
        x = math.atan2(-R[1,2], R[1,1])
        y = math.atan2(-R[2,0], sy)
        z = 0
    return np.array([x, y, z])



def extract_metadata(self):
    self.metaData = {}
    exif_info = self.image._getexif()
    if exif_info:
        print("Found Meta Data!","\n")
        for (tag, value) in exif_info.items():
            tagname = TAGS.get(tag,tag)
            self.metaData[tagname] = value
        self.focal_length = int(self.metaData['FocalLength'][0]/self.metaData['FocalLength'][1])*self.imagew/35.9
        print("Focal :", np.round(self.focal_length), " pixels")
        self.cameraMatrix = np.array([[self.focal_length,0,self.imagew/2],[0,self.focal_length,self.imageh/2],[0,0,1]])

这是我的结果：姿势（北东仰角横滚俯仰偏航）：

(4221680.42,2006518.54,-4807966,83.96,-47.243,34.061)

Answer 1

要使用solvePnPRansac，您需要事先完成相机校准。 相机校准将为您提供相机矩阵和相机的失真系数。 我可以看到你只是作为一个空数组作为失真系数传递。 如果没有相机校准，您将无法从姿势估计中获得任何准确性。 转到 OpenCV 文档进行相机校准。

一旦你校准了你的相机，你就可以使用solvePnPRansac的相机矩阵和失真系数。

你提到你有你的焦距，但这有多准确？ 你从哪里得到的？ 此外，您似乎假设光学中心位于图像的中心，但情况可能不一定如此。