Pose Estimation with SolvePNP

Question

I am attempting to solve for the pose of several time-lapse cameras that are overlooking a glacier in Alaska. So far, all estimated pose's have been wildly inaccurate. The units I am using for real world coordinates are UTM's (Easting/Northing) and elevation. I beleive my issue lies in a units error, or a mismatch in world coordinates to image coordinates.

Here is what I have for focal length and Ground Control:

Focal : 5740.0 pixels

World: [3.93610609e+05 6.69557833e+06 7.82287000e+02] Image: [ 479. 2448.]

World: [3.93506713e+05 6.69585564e+06 9.61337000e+02] Image: [ 164. 1398.]

World: [3.94569509e+05 6.69555068e+06 6.21075000e+02] Image: [2812. 3853.]

World: [3.97774e+05 6.69986e+06 1.64200e+03] Image: [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 = [email protected]

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]])

Here are my results: pose (northing easting elevation roll pitch yaw):

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

guivi guivi · Accepted Answer · 2020-02-11T10:14:18

To use solvePnPRansac you need to have done a camera calibration previously. The camera calibration will give you the camera matrix and the distortion coefficients of the camera. I can see you are just passing as a null array as distortion coefficients. Without camera calibration you will not get any accuracy from pose estimation. Go to OpenCV documentation for camera calibration.

Once you have your camera calibrated you will be able to use the camera matrix and distortion coefficients for the solvePnPRansac.

You mentioned you have your focal length, but how accurate is this? where did you got it from? Also, you seem to presume that the optical centre is on the centre of the image but this may not be necessarily the case.

Pose Estimation with SolvePNP

1 Answers