基于pnp的姿态估计

Question

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

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

焦距：5740.0像素

世界：[3.93610609e+056.69557833e+067.82287000e+02]图片：[479.2448]

世界：[3.93506713e+056.69585564e+069.61337000e+02]图像：[164.1398]

世界：[3.94569509e+056.69555068e+066.21075000e+02]图像：[2812.3853]

世界：[3.97774e+056.69986e+061.64200e+03]图像：[6310.1398]

self.tvec=（393506.7136695855.641961.337） self.rvec=（np.弧度（0），np.弧度（0），np.弧度（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.422006518.54，-48079683.96，-47.243,34.061）

Answer 1

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

校准相机后，您将能够使用solvePnPRansac的相机矩阵和失真系数

你说过你有焦距，但这有多准确？你从哪里弄来的？此外，您似乎假定光学中心位于图像的中心，但情况未必如此