def LoadTransformationGogma(transformationPathGogma):
with open(transformationPathGogma) as f:
x = np.loadtxt(f)
print x
if x.size > 12:
x = x[0,:]
t_abGlobal = x[:3]
R_abGlobal = np.reshape(x[3:],(3,3))
print "R_ab",R_abGlobal
print "t_ab",t_abGlobal
q_baGlobal = Quaternion()
q_baGlobal.fromRot3(R_abGlobal.T)
transformationPathGogmaRefined = os.path.splitext(transformationPathGogma)[0]+"_refinement.txt"
print transformationPathGogmaRefined
with open(transformationPathGogmaRefined) as f:
x = np.loadtxt(f)
print x
if x.size > 12:
x = x[0,:]
t_ab = x[:3]
R_ab = np.reshape(x[3:],(3,3))
print "R_ab",R_ab
print "t_ab",t_ab
q_ba = Quaternion()
q_ba.fromRot3(R_ab.T)
return q_ba, -R_ab.T.dot(t_ab), q_baGlobal, -R_abGlobal.T.dot(t_abGlobal),
def LoadTransformationGoICP(transformationPathGoICP):
with open(transformationPathGoICP) as f:
dt = float(f.readline())
R = np.zeros((3,3))
for i in range(3):
Ristring = f.readline()[:-1].split(" ")
print Ristring
R[i,:] = np.array([float(Ri) for Ri in Ristring if len(Ri) > 1])
t = np.zeros(3)
for i in range(3):
t[i] = float(f.readline()[:-1])
print "R",R
print "t",t
print "dt",dt
# R = R.T
# t = - R.dot(t)
q = Quaternion()
q.fromRot3(R.T)
return q, -R.T.dot(t), dt
import numpy as np
from scipy.linalg import solve
from js.geometry.quaternion import Quaternion
for path in [ "../data/gazebo_winter/", "../data/mountain_plain/", "../data/gazebo_summer/" ]:
#for path in [ "../data/stairs/", "../data/apartment/", "../data/wood_summer/" ]:
with open(path+"pose_scanner_leica.csv") as f:
f.readline()
x = np.loadtxt(f,delimiter=",")
for i in range(x.shape[0]):
T_wc = np.reshape(x[i,2:],(4,4))
R_wc = T_wc[:3,:3]
q_wc = Quaternion()
q_wc.fromRot3(R_wc)
t_wc = T_wc[:3,3]
print t_wc, q_wc
with open(path+"pose_{}.csv".format(i),"w") as fout:
fout.write("q_w q_x q_y q_z t_x t_y t_z\n")
fout.write("{} {} {} {} {} {} {}".format(q_wc.q[0],\
q_wc.q[1],q_wc.q[2],q_wc.q[3],t_wc[0],t_wc[1],t_wc[2]))