Exemplo n.º 1
0
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), 
Exemplo n.º 2
0
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
Exemplo n.º 3
0
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
Exemplo n.º 4
0
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
Exemplo n.º 5
0
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
Exemplo n.º 6
0
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
Exemplo n.º 7
0
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
Exemplo n.º 8
0
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
Exemplo n.º 9
0
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
Exemplo n.º 10
0
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
Exemplo n.º 11
0
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
Exemplo n.º 12
0
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
Exemplo n.º 13
0
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
Exemplo n.º 14
0
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]))
Exemplo n.º 16
0
  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)