ReprojectPoints.py

# -*- coding: utf-8 -*-
"""
Created on Wed Jun 26 08:42:01 2019

@author: thomas.drevet
"""

# Import the libraries
import numpy as np
import cv2
from DéFI.Orthorectification.ReadTXT import ReadIntParam
from DéFI.Orthorectification.ReadODS import ReadMiresCoord, ReadMiresImag
import matplotlib.pyplot as plt

pathMiresCoordODS = "C:\\Users\\thomas.drevet\\Documents\\Seafile\\DeFI\\Donnees\\Donnees_Georeferencement\\coordonnees-mires-saintemarie-2019.ods"
IntMtx, DistCoef = ReadIntParam('DéFI\\Orthorectification\\CameraCalibration\\CalibCam2.txt')

#DistCoef = np.zeros(DistCoef.shape)

pathMiresImageODS = "C:\\Users\\thomas.drevet\\Documents\\Seafile\\DeFI\\Donnees\\Donnees_Georeferencement\\GéoMires\\camera2\\IMAG1674.ods"

MiresCoord = ReadMiresCoord(pathMiresCoordODS)
MiresImag = ReadMiresImag(pathMiresImageODS)

ObjectCoord = []
ObjectImag = []

# For all image references...
for i in range (len(MiresImag)):
    # ... Find its corresponding world references...
    for j in range(len(MiresCoord)):
        if MiresImag[i]['IDMires'] == MiresCoord[j]['NumeroPoint']:
            NewObjectCoord = [MiresCoord[j]['X'], MiresCoord[j]['Y'], MiresCoord[j]['Z']]
            ObjectCoord.append(NewObjectCoord)
            NewObjectImag = [np.float(MiresImag[i]['xImg']), np.float(MiresImag[i]['yImg'])]
            ObjectImag.append(NewObjectImag)
        # ... And store their coordinates
        ObjCoord = np.array(ObjectCoord)
        ObjImag = np.array(ObjectImag)

ResSupMires = []

for k in range(ObjImag.shape[0]):
    ObjCoordR = np.delete(ObjCoord, (k), axis=0)
    ObjImagR = np.delete(ObjImag, (k), axis=0)

    _, ERotVec, ETranVec = cv2.solvePnP(ObjCoordR, ObjImagR, IntMtx, DistCoef)
    _, RotVec, TranVec = cv2.solvePnP(ObjCoordR, np.ascontiguousarray(ObjImagR[:,:2]).reshape((len(ObjImagR),1,2)), IntMtx, DistCoef, ERotVec, ETranVec, True, cv2.SOLVEPNP_EPNP)
    
    
    ProjPt = []
    
    for i in range(ObjImagR.shape[0]):
        NormProjPt = cv2.projectPoints(ObjCoordR[i].reshape(-1,3), RotVec, TranVec, IntMtx, DistCoef)[0][0, 0]
        ProjPt.append(NormProjPt)
    ReProjPts = np.array(ProjPt)
    
    Diff = ReProjPts - ObjImagR
    SumPDiff = np.power(Diff[:, 0], 2) + np.power(Diff[:, 1], 2)
    Res = np.sqrt(SumPDiff[:])
    MRes = np.mean(Res)
    ResSupMires.append(MRes)