<jupyter_start><jupyter_text>Test SolvePNP<jupyter_code>import os
import sys
sys.path.append("..")
import matplotlib.pyplot as plt
import matplotlib.image as mpimg
import cv2
import pandas as pd
import numpy as np
from math import sin, tan, cos, pi, radians, degrees

def cMatrix(fLength,cx,cy):
    return np.array([[fLength,0,cx,0],[0,fLength,cy,0],[0,0,1,0]])

def eulerAnglesToRotationMatrix(theta) :
     
    R_x = np.array([[1,         0,                  0                   ],
                    [0,         cos(theta[0]), -sin(theta[0]) ],
                    [0,         sin(theta[0]), cos(theta[0])  ]
                    ])
         
         
                     
    R_y = np.array([[cos(theta[1]),    0,      sin(theta[1])  ],
                    [0,                1,      0                   ],
                    [-sin(theta[1]),   0,      cos(theta[1])  ]
                    ])
                 
    R_z = np.array([[cos(theta[2]),    -sin(theta[2]),    0],
                    [sin(theta[2]),    cos(theta[2]),     0],
                    [0,                     0,                      1]
                    ])
                     
                     
    R = np.dot(R_z, np.dot( R_y, R_x ))
 
    return R
objX=[]
objY=[]
objZ=[]

imagePoints = pd.read_csv('../data/interim/2dprojpts530.csv',sep=',', skiprows=[1,2])
imagePoints['X'] = imagePoints['X'].astype('float32')
imagePoints['Y'] = imagePoints['Y'].astype('float32')
imagePointVal = imagePoints.iloc[:,1:].values

MCapVal = pd.read_pickle('../data/processed/01.pickle')
MCapFrame = MCapVal.iloc[2400]
MCapOrdered = MCapFrame.reindex(imagePoints['Name'].tolist()).dropna()

for i in range(len(MCapOrdered)):
    objX.append(MCapOrdered[i][0][0])
    objY.append(MCapOrdered[i][1][0])
    objZ.append(MCapOrdered[i][2][0])

objMat = np.array((objX,objY,objZ))    
cameraMatrix = cMatrix(267.3,352/2,240/2)
cameraMatrix=cameraMatrix[:,:-1]

rotx = radians(0)
roty = radians(50)
rotz = radians(0)
transx = -40
transy = -10
transz = 30

distCoeffs=0
rvec = np.array([rotx,roty,rotz])
tvec = np.array([transx,transy,transz]).astype('float32')

pnpVal = cv2.solvePnP(
    objMat.T,
    imagePointVal,
    cameraMatrix,
    distCoeffs,
    rvec,
    tvec,
    useExtrinsicGuess=1)
rMat = eulerAnglesToRotationMatrix(pnpVal[1])
tvec = pnpVal[2]
wMat = np.c_[rMat,tvec]

projMCap = []

for i in range(MCapFrame.shape[0]):
    subMCap = MCapFrame.iloc[i]
    subMCap = np.append(subMCap,[1.])
    projMCap.append(np.linalg.multi_dot([cameraMatrix,wMat,subMCap]))

projMCap = pd.DataFrame(projMCap,columns = ['x','y','z'], index=MCapFrame.keys())
projMCap['x'] = projMCap['x']/projMCap['z']
projMCap['y'] = projMCap['y']/projMCap['z']

projMCap = projMCap.drop(columns = ['z'])
Vidframe=530

fig = plt.figure(figsize=(20,20))
ax = fig.add_subplot(111)

vidcap = cv2.VideoCapture('../data/raw/01_01.mpg')
vidcap.set(cv2.CAP_PROP_POS_FRAMES,Vidframe)
success, image = vidcap.read()

ax.imshow(image,aspect='equal', extent=[0,vidcap.get(3),0,vidcap.get(4)])

ax.scatter(projMCap['x'],projMCap['y'], color='r')

projMCap.to_pickle('../data/processed/01proj.pickle')
pd.read_pickle('../data/processed/01proj.pickle')<jupyter_output><empty_output>
{'fraction_non_alphanumeric': 0.17743940335612182, 'fraction_numerical': 0.035736482287134864, 'mean_word_length': 2.6579545454545452, 'pattern_counts': {'":': 0, '<': 5, '<?xml version=': 0, '>': 5, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 6, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '1', 'dir': 'jupyter-structured-clean-dedup', 'id': '580745', 'license': 'MIT', 'n_tokens_mistral': 1310, 'n_tokens_neox': 1240, 'n_words': 176, 'path': 'notebooks/2D_Projection_DK.ipynb', 'repo_name': 'kamkard/AMCParser', 'size': '3218'}