bug.py

# -*- coding: utf-8 -*-
"""
pos to calibed pos
Created on Wed Oct  3 23:03:48 2018

@author: akira
"""
#To serve the data run python -m http.server 8000 

import numpy as np
import pickle
import cv2
import matplotlib.pyplot as plt
import math
from pyquaternion import Quaternion
from mpl_toolkits.mplot3d import axes3d

camera_mat = np.loadtxt('K.csv', delimiter=',')
dist_coef = np.loadtxt('d.csv', delimiter=',')
with open('pos.pickle', mode='rb') as f:
    d=pickle.load(f)
poss=d["pos"]
valid=[ a>0.1 for a in d["level"]]

tans=[]
for i in range(len(poss)):
    tans.append(cv2.undistortPoints(np.array([poss[i]]).astype(np.float),camera_mat,dist_coef)[0])

plt.figure("poss")
plt.clf()
for i in range(len(tans)):
    plt.plot(poss[i][valid[i],0],poss[i][valid[i],1],"o")
    

plt.figure("tans")
plt.clf()
for i in range(len(tans)):
    plt.plot(180/math.pi*np.arctan(tans[i][valid[i],0]),180/math.pi*np.arctan(tans[i][valid[i],1]),"o")
    


h=1.5 #height is z
q=Quaternion(axis=[1.0, 0.00, 0.0], angle=math.pi/4) ## angle 0　means Down
def doRot(h,q,t):
    t=np.array( (t[0],t[1],1))
    t=q.rotate(t)
    t*=h/t[2]
    return t
def doRotR(q,t):
    t=np.array( (t[0],t[1],1))
    t=q.rotate(t)
    return t
    

cp=[]
t=None
for i in range(len(tans)):
    cp.append([])
    for j in range(tans[i].shape[0]):
        cp[-1].append(doRot(h,q,tans[i][j,:]))
    cp[-1]=np.array(cp[-1])

plt.figure("caribpos")
plt.clf()


plt.axes().set_aspect('equal', 'datalim')


for i in range(len(cp)):
    plt.plot(cp[i][valid[i],0],cp[i][valid[i],1],"o")
    




fig = plt.figure("3D")
plt.clf()
ax = fig.add_subplot(111, projection='3d')
ax.set_aspect('equal', 'datalim')



D=np.array(([0,0,0], doRot(h,q,[0,0]))).transpose()
ax.plot(D[0],D[1],D[2]*-1,'k-')
wd=math.tan(70*math.pi/180)
hd=math.tan(45*math.pi/180)
D=np.array((doRot(h,q,[wd,hd]) ,[0,0,0] ,doRot(h,q,[-wd,hd]),[0,0,0],doRot(h,q,[-wd,-hd]),[0,0,0],doRot(h,q,[wd,-hd]) ,[0,0,0],doRot(h,q,[wd,hd])          )  ).transpose()
ax.plot(D[0],D[1],D[2]*-1,'k-')
D=np.array((doRot(h,q,[wd,hd]) ,doRot(h,q,[-wd,hd]),doRot(h,q,[-wd,-hd]),doRot(h,q,[wd,-hd]) ,doRot(h,q,[wd,hd]))  ).transpose()
ax.plot(D[0],D[1],D[2]*-1,'k-')
D=np.array((doRotR(q,[0,0]) ,doRotR(q,[wd,hd]) ,doRotR(q,[-wd,hd]),doRotR(q,[-wd,-hd]),doRotR(q,[wd,-hd]) ,doRotR(q,[wd,hd]))  ).transpose()
ax.plot(D[0],D[1],D[2]*-1,'k-')


for i in range(len(cp)):
    ax.plot(cp[i][valid[i],0],cp[i][valid[i],1],cp[i][valid[i],2]*-1,".")




    
import setting
import json
with open("pos.json","w") as fp:
    json.dump( {"level":[a.tolist() for a in  d["level"]],"pos":[ a.tolist() for a in cp],"host":setting.poshosts},fp)