使用 SolvePNP 进行姿势估计

Posted 2023-02-19

【中文标题】使用 SolvePNP 进行姿势估计

【英文标题】：Pose Estimation with SolvePNP

【发布时间】：2020-05-26 09:38:56

【问题描述】：

我正在尝试解决几个俯瞰阿拉斯加冰川的延时摄影机的姿势。到目前为止，所有估计的姿势都非常不准确。 我用于真实世界坐标的单位是 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)

【参考方案1】：

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

校准好相机后，您就可以将相机矩阵和失真系数用于solvePnPRansac。

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

【讨论】：

guivi，我还没有校准相机。焦距的计算方法是从图像元数据（28 mm，使用 extract_metadata()）中获取信息，然后转换为像素尺寸，28*image_width/sensor_width。

你有什么理由怀疑这是不正确的吗？

这可能是一个很好的近似值，但我建议您校准相机。另外，我没有看到图像，但您没有使用任何我不建议您使用的失真参数。