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 RunFFT(scanApath, scanBpath, transformationPathFFT, q_gt=None,
t_gt=None, returnBoth=False):
scanApathAbs = os.path.abspath(scanApath)
scanBpathAbs = os.path.abspath(scanBpath)
transformationPathFFTAbs = os.path.abspath(transformationPathFFT)
args = ["./runMatlabFFT.sh",
'\'' + scanApathAbs + '\'',
'\'' + scanBpathAbs + '\'',
'\'' + transformationPathFFTAbs+'\'']
print " ".join(args)
t0 = time.time()
err = subp.call(" ".join(args), shell=True)
dt = time.time() - t0
print "dt: ", dt, "[s]"
if err > 0:
print "ERROR in run FFT"
return Quaternion(), np.zeros(3), dt, False
qs,ts = LoadTransformation(transformationPathFFT)
id_best = 0
if not (q_gt is None and t_gt is None):
dist = np.zeros(len(qs))
for i in range(len(qs)):
dist[i] = q_gt.angleTo(qs[i])
print dist
id_best = np.argmin(dist)
if returnBoth:
return qs[id_best], ts[id_best], qs[0], ts[0], dt, True
else:
return qs[id_best], ts[id_best], dt, True
def RunBBsimple(scanApath, scanBpath, transformationPathBB, lambdaS3,
lambdaR3, EGImode=False):
args = ['../pod-build/bin/dpOptTransPly',
'-a {}'.format(scanApath),
'-b {}'.format(scanBpath),
'-l {}'.format(lambdaS3),
'-t {}'.format(lambdaR3),
'-o ' + re.sub(".csv","",transformationPathBB)
]
if EGImode:
args.append('-e')
print " ".join(args)
t0 = time.time()
err = subp.call(" ".join(args), shell=True)
dt = time.time() - t0
if err > 0:
print "ERROR in RunBB"
return Quaternion(), np.zeros(3), np.zeros(4), dt, False
q,t,lbS3,lbR3, KvmfA, KvmfB, KgmmA, KgmmB =\
LoadTransformationAndBounds(transformationPathBB)
Ks = np.array([KvmfA, KvmfB, KgmmA, KgmmB])
print "dt: ", dt, "[s]"
if np.logical_or(np.isinf([lbR3]), np.isnan([lbR3])).all():
return q, np.array([np.nan, np.nan, np.nan]), Ks,dt,False
return q,t, Ks, dt,True
def LoadTransformation(transformationPath):
with open(transformationPath) as f:
f.readline()
qt = np.loadtxt(f)
if len(qt.shape) == 1:
q = Quaternion(w=qt[0], x=qt[1], y=qt[2], z=qt[3])
t = qt[4:7]
else:
q = []
t = []
for i in range(qt.shape[0]):
q.append(Quaternion(w=qt[i,0], x=qt[i,1], y=qt[i,2], z=qt[i,3]))
t.append(qt[i,4:7])
print "from path: ", transformationPath
print 'q', q
print 't', t
return q,t
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
def RunBB(cfg, scanApath, scanBpath, transformationPathBB,\
EGImode=False, TpSmode=False, AAmode=False, outputBoundsAt0=False):
lbsS3 = np.zeros(len(cfg["lambdaS3"]))
lbsR3 = np.zeros(len(cfg["lambdaS3"]))
Ks = np.zeros((len(cfg["lambdaS3"]), 4))
qs, ts = [],[]
dt = 0.
for j,lambdaS3 in enumerate(cfg["lambdaS3"]):
args = ['../pod-build/bin/dpOptTransPly',
'-a {}'.format(scanApath),
'-b {}'.format(scanBpath),
'-l {}'.format(lambdaS3),
'-t {}'.format(cfg["lambdaR3"]),
'-o ' + re.sub(".csv","",transformationPathBB)
]
if EGImode:
args.append('-e')
if TpSmode:
args.append('--TpS')
if AAmode:
args.append('--AA')
if outputBoundsAt0:
args.append('--oB0')
print " ".join(args)
t0 = time.time()
err = subp.call(" ".join(args), shell=True)
dt += time.time() - t0
if err > 0:
print "ERROR in RunBB"
return Quaternion(), np.zeros(3), np.zeros(4), dt, False
q,t,lbsS3[j],lbsR3[j], KvmfA, KvmfB, KgmmA, KgmmB =\
LoadTransformationAndBounds(transformationPathBB)
qs.append(q)
ts.append(t)
Ks[j,0], Ks[j,1], Ks[j,2], Ks[j,3] = KvmfA, KvmfB, KgmmA, KgmmB
print 'lbsS3', lbsS3
print 'lbsR3', lbsR3
print "dt: ", dt, "[s]"
if np.logical_or(np.isinf(lbsR3), np.isnan(lbsR3)).all():
idMax = np.argmax(lbsS3)
return qs[idMax], np.array([np.nan, np.nan, np.nan]), Ks[idMax,:],dt,False
idMax = np.argmax(lbsS3*lbsR3)
print "choosing scale {} of run {}".format(cfg["lambdaS3"][idMax],idMax)
q,t = qs[idMax], ts[idMax]
lbS3, lbR3 = lbsS3[idMax], lbsR3[idMax]
KvmfA, KvmfB, KgmmA, KgmmB = Ks[idMax,0], Ks[idMax,1], Ks[idMax,2], Ks[idMax,3]
with open(transformationPathBB,'w') as f: # write best one back to file
f.write("qw qx qy qz tx ty tz lbS3 lbR3 KvmfA KvmfB KgmmA KgmmB\n")
f.write("{} {} {} {} {} {} {} {} {} {} {} {} {}\n".format(
q.q[0],q.q[1],q.q[2],q.q[3],t[0],t[1],t[2],lbS3,lbR3,KvmfA,\
KvmfB, KgmmA, KgmmB))
return q,t, Ks[idMax,:], dt,True
def LoadTransformationAndData(transformationPath):
with open(transformationPath) as f:
f.readline()
qt = np.loadtxt(f)
if len(qt.shape) == 1:
q = qt[:4]
t = qt[4:7]
data = qt[7:]
else:
q = qt[0,:4]
t = qt[0,4:7]
data = qt[0,:]
q = Quaternion(w=q[0], x=q[1], y=q[2], z=q[3])
return q,t,data
def RunGogma(scanApath, scanBpath, transformationPathGogma,
pathGOGMAcfg = '/home/jstraub/workspace/research/3rdparty/gogma/build/config.txt',
timeout_sec = 3600):
PreparePcForGogma(scanApath, scanBpath)
args = ['/home/jstraub/workspace/research/3rdparty/gogma/build/gogma',
os.path.abspath(re.sub(".ply",".txt",scanApath)),
os.path.abspath(re.sub(".ply",".txt",scanBpath)),
pathGOGMAcfg,
os.path.abspath(re.sub(".csv",".txt",transformationPathGogma)),
"2>&1"
]
print " ".join(args)
t0 = time.time()
err = run(" ".join(args), timeout_sec)
t1 = time.time()
dt = t1 - t0
print "error ", err
if err == 0:
# err = subp.call(" ".join(args), shell=True)
q_ba,t_ba,q_baGlobal,t_baGlobal = LoadTransformationGogma(re.sub(".csv",
".txt",transformationPathGogma))
return q_ba,t_ba,q_baGlobal,t_baGlobal,dt,True
else:
return Quaternion(),np.array([0,0,0]),Quaternion(),np.array([0,0,0]),dt,False
def RunMM(scanApath, scanBpath, transformationPathMM):
args = ['../pod-build/bin/moment_matched_T3',
'-a {}'.format(scanApath),
'-b {}'.format(scanBpath),
'-o {}'.format(re.sub(".csv","",transformationPathMM))
]
print " ".join(args)
t0 = time.time()
err = subp.call(" ".join(args), shell=True)
dt = time.time() - t0
print "dt: ", dt, "[s]"
if err > 0:
print "ERROR in run MM"
return Quaternion(), np.zeros(3), dt, False
q,t = LoadTransformation(transformationPathMM)
return q,t, dt,True
def LoadTransformationAndBounds(transformationPath):
with open(transformationPath) as f:
f.readline()
qt = np.loadtxt(f)
if len(qt.shape) == 1:
q = qt[:4]
t = qt[4:7]
lbS3 = qt[7]
lbR3 = qt[8]
KvmfA, KvmfB, KgmmA, KgmmB = qt[9],qt[10],qt[11],qt[12]
else:
q = qt[0,:4]
t = qt[0,4:7]
lbS3 = qt[0,7]
lbR3 = qt[0,8]
KvmfA, KvmfB, KgmmA, KgmmB = qt[0,9],qt[0,10],qt[0,11],qt[0,12]
print 'q', q
print 't', t
q = Quaternion(w=q[0], x=q[1], y=q[2], z=q[3])
return q,t, lbS3, lbR3, KvmfA, KvmfB, KgmmA, KgmmB
def RunICP(scanApath, scanBpath, transformationPathICP,
useNormals=True, transformationPathGlobal=None):
args = ['../pod-build/bin/icp_T3',
'-a {}'.format(scanApath),
'-b {}'.format(scanBpath),
'-o {}'.format(re.sub(".csv","",transformationPathICP))
]
if not transformationPathGlobal is None:
args.append('-t {}'.format(transformationPathGlobal))
if useNormals:
args.append("-n")
print " ".join(args)
t0 = time.time()
err = subp.call(" ".join(args), shell=True)
dt = time.time() - t0
print "dt: ", dt, "[s]"
if err > 0:
print "ERROR in run ICP"
return Quaternion(), np.zeros(3), dt,False
q,t = LoadTransformation(transformationPathICP)
return q,t,dt,True
def RunFFT(scanApath, scanBpath, transformationPathFFT, q_gt=None,
t_gt=None, returnBoth=False,
timeout_sec = 3600
):
scanApathAbs = os.path.abspath(scanApath)
scanBpathAbs = os.path.abspath(scanBpath)
transformationPathFFTAbs = os.path.abspath(transformationPathFFT)
args = ["./runMatlabFFT.sh",
'\'' + scanApathAbs + '\'',
'\'' + scanBpathAbs + '\'',
'\'' + transformationPathFFTAbs+'\'']
print " ".join(args)
t0 = time.time()
err = run(" ".join(args), timeout_sec)
dt = time.time() - t0
print "dt: ", dt, "[s]"
if not err == 0:
print "ERROR in run FFT"
return Quaternion(), np.zeros(3), dt, False
qs,ts,raw = LoadTransformation(transformationPathFFT)
if not isinstance(qs, (list, tuple)):
qs = [qs]
ts = [ts]
id_best = 0
if not (q_gt is None and t_gt is None):
dist = np.zeros(len(qs))
for i in range(len(qs)):
dist[i] = q_gt.angleTo(qs[i])
print dist
id_best = np.argmin(dist)
if len(qs)==1:
if np.all(ts[0] == 0):
print "translation 0 => FFT failed"
return qs[0], ts[0], dt, False
if returnBoth:
return qs[id_best], ts[id_best], qs[0], ts[0], dt, True
else:
return qs[id_best], ts[id_best], dt, True
def RunGoICP(scanApath, scanBpath, transformationPathGoICP,
NdDownsampled=3000, timeout_sec = 3600):
scale = PreparePcForGoICP(scanApath, scanBpath)
args = ['../goIcp/src/build/GoICP',
re.sub(".ply",".txt",scanApath),
re.sub(".ply",".txt",scanBpath),
'{}'.format(NdDownsampled),
'../goIcp/config.txt',
re.sub(".csv",".txt",transformationPathGoICP)
]
print " ".join(args)
t0 = time.time()
err = run(" ".join(args), timeout_sec)
dt = time.time() - t0
print "error ", err
print "dt: ", dt, "[s]"
# err = subp.call(" ".join(args), shell=True)
if err == 0:
q,t,_ = LoadTransformationGoICP(re.sub(".csv",".txt",transformationPathGoICP))
t*= scale
return q,t,dt,True
else:
return Quaternion(),np.array([0,0,0]),dt,False
version = "3.06" # early termination of rotations to get multiple peaks (lvl 11)
version = "3.07" # eval on 3Dsim data
validInput = False
if re.search("config_[0-9]+.txt", cmdArgs.input):
with open(cmdArgs.input) as f:
scanApath = os.path.join(
os.path.split(cmdArgs.input)[0],
f.readline()[:-1])
scanBpath = os.path.join(
os.path.split(cmdArgs.input)[0],
f.readline()[:-1])
f.readline()
x = np.loadtxt(f)
# T_gt is T_ba but the saved config is T_ab
q_gt = Quaternion(w=x[3], x=x[0], y=x[1], z=x[2]).inverse()
t_gt = -q_gt.rotate(x[4:7])
data_gt = x[7:]
paramString = os.path.splitext(os.path.split(cmdArgs.input)[1])[0]
print paramString
q_A = Quaternion(1, 0, 0, 0)
t_A = np.array([0, 0, 0])
q_B = Quaternion(1, 0, 0, 0)
t_B = np.array([0, 0, 0])
if os.path.isfile(scanApath) and os.path.isfile(scanBpath):
validInput = True
else:
validInput = False
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]))
R_BA = R_B.dot(R_A.T)
t_BA = -R_BA.dot(t_A) + t_B
results = {"GT":{"q":q_gt.q.tolist(), "t":t_gt.tolist(),
"q_A_W":q_A.q.tolist(), "t_A_W":t_A.tolist(),
"q_B_W":q_B.q.tolist(), "t_B_W":t_B.tolist(),
"overlap":data_gt[0], "sizeA":data_gt[1], "sizeB": data_gt[2],
# compute magnitude of translation as well as rotation of the two
# viewpoints of the scene.
"dtranslation": np.sqrt((t_BA**2).sum()),
"dangle": q_A.angleTo(q_B)*180./np.pi
},
"version":version}
if showUntransformed:
q0 = Quaternion(1.,0.,0.,0.)
DisplayPcs(scanApath, scanBpath, q0, np.zeros(3), False, False)
DisplayPcs(scanApath, scanBpath, q_gt,t_gt, True, True)
if runGoICP:
q,t,dt,success = RunGoICP(scanApath, scanBpath, transformationPathGoICP)
if not success:
err_a, err_t = np.nan, np.nan
else:
err_a, err_t = EvalError(q_gt, t_gt, q, t)
print "GoICP: {} deg {} m".format(err_a, err_t)
results["GoICP"] = {"err_a":err_a, "err_t":err_t, "q":q.q.tolist(),
"t":t.tolist(), "dt":dt}
q0 = Quaternion(1.,0.,0.,0.)
DisplayPcs(scanApath, scanBpath, q0, np.zeros(3), False, False, False)
