Skip to content

Commit

Permalink
- Added two new datasets, ring and cityRing. They are used in the ICR…
Browse files Browse the repository at this point in the history 
…A13 paper on

  Switchable Constraints vs. Max Mixtures vs. RRR. 
- Some improvements to generateDataset.py
  • Loading branch information
suenderhauf committed Mar 6, 2013
1 parent 4271f09 commit f5de381
Show file tree
Hide file tree
Showing 9 changed files with 18,730 additions and 35 deletions.
5,622 changes: 5,622 additions & 0 deletions datasets/cityRing/groundTruth/cityRing.g2o
View file

Large diffs are not rendered by default.

2,360 changes: 2,360 additions & 0 deletions datasets/cityRing/groundTruth/edges.txt
View file

Large diffs are not rendered by default.

2,361 changes: 2,361 additions & 0 deletions datasets/cityRing/groundTruth/poses.txt
View file

Large diffs are not rendered by default.

5,622 changes: 5,622 additions & 0 deletions datasets/cityRing/originalDataset/cityRing.g2o
View file

Large diffs are not rendered by default.

147 changes: 112 additions & 35 deletions datasets/generateDataset.py
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
#!/usr/bin/python

# This is part of Vertigo.
# This is part of the Vertigo suite.
# Niko Suenderhauf
# Chemnitz University of Technology
# [email protected]
Expand Down Expand Up @@ -37,9 +37,14 @@ def checkOptions(options):
print "Dataset to read (--in) must be given."
return False

if (options.information.count(",") != 0) and (options.information.count(",") != 5) and (options.information.count(",") != 20):
print "Information matrix must be given in full upper-triangular form. E.g. --information=42,0,0,42,0,42 or as a single value that is used for all diagonal entries, e.g. --information=42."
return False
if options.information == "":
options.information=None


if options.information:
if (options.information.count(",") != 0) and (options.information.count(",") != 5) and (options.information.count(",") != 20):
print "Information matrix must be given in full upper-triangular form. E.g. --information=42,0,0,42,0,42 or as a single value that is used for all diagonal entries, e.g. --information=42."
return False



Expand Down Expand Up @@ -110,7 +115,7 @@ def euler_to_quat(yaw, pitch, roll):


# ==================================================================
def writeDataset(filename, vertices, edges, mode, outliers=0, switchPrior=1, switchSigma=1, maxmixWeight=10e-8, groupSize=1, doLocal=0, informationMatrix="42,0,0,42,0,42", doSwitchable=True, doMaxMix=False):
def writeDataset(filename, vertices, edges, mode, outliers=0, switchPrior=1, switchSigma=1, maxmixWeight=10e-12, maxmixScale=0.01, groupSize=1, doLocal=0, informationMatrix="42,0,0,42,0,42", doSwitchable=True, doMaxMix=False, doMaxMixAgarwal=False, perfectMatch=False):


switchInfo=1.0/switchSigma**2
Expand All @@ -124,31 +129,42 @@ def writeDataset(filename, vertices, edges, mode, outliers=0, switchPrior=1, swi
# edges and vertices are called differently, depending on 2D or 3D mode ...
if mode == 2:
edgeStr='EDGE_SE2'
if informationMatrix.count(",")==0:
# if there is only a single value, convert it into full upper triangular form with that value on the diagonal
try:
diagEntry=float(informationMatrix)
except:
print "! Invalid value for information matrix. If you give only a single value, it must be a number, e.g. --information=42"
return False
informationMatrix = "%f,0,0,%f,0,%f" % (diagEntry,diagEntry,diagEntry)

# check entries for information matrix for additional loop closure constraints (outliers)
if not informationMatrix:
print "Determining information matrix automatically..."
else:
if informationMatrix.count(",")==0:
# if there is only a single value, convert it into full upper triangular form with that value on the diagonal
try:
diagEntry=float(informationMatrix)
except:
print "! Invalid value for information matrix. If you give only a single value, it must be a number, e.g. --information=42"
return False
informationMatrix = "%f,0,0,%f,0,%f" % (diagEntry,diagEntry,diagEntry)

elif informationMatrix.count(",")!=5:
print "! Invalid number of entries in information matrix. Full upper triangular form has to be given, e.g. --information=42,0,0,42,0,42."
return False
elif informationMatrix.count(",")!=5:
print "! Invalid number of entries in information matrix. Full upper triangular form has to be given, e.g. --information=42,0,0,42,0,42."
return False

elif mode == 3:
edgeStr='EDGE_SE3'
if informationMatrix.count(",")==0:
# if there is only a single value, convert it into full upper triangular form with that value on the diagonal
try:
diagEntry=float(informationMatrix)
except:
print "! Invalid value for information matrix. If you give only a single value, it must be a number, e.g. --information=42"

# check entries for information matrix for additional loop closure constraints (outliers)
if not informationMatrix:
print "Determining information matrix automatically"
else:
if informationMatrix.count(",")==0:
# if there is only a single value, convert it into full upper triangular form with that value on the diagonal
try:
diagEntry=float(informationMatrix)
except:
print "! Invalid value for information matrix. If you give only a single value, it must be a number, e.g. --information=42"
return False
informationMatrix = "%f,0,0,0,0,0,%f,0,0,0,0,%f,0,0,0,%f,0,0,%f,0,%f" % (diagEntry,diagEntry,diagEntry,diagEntry, diagEntry, diagEntry)
elif informationMatrix.count(",")!=20:
print "! Invalid number of entries in information matrix (--information). Full upper triangular form has to be given."
return False
informationMatrix = "%f,0,0,0,0,0,%f,0,0,0,0,%f,0,0,0,%f,0,0,%f,0,%f" % (diagEntry,diagEntry,diagEntry,diagEntry, diagEntry, diagEntry)
elif informationMatrix.count(",")!=20:
print "! Invalid number of entries in information matrix (--information). Full upper triangular form has to be given."
return False
else:
print "! Invalid mode. It must be either 2 or 3 but was", mode
return False
Expand All @@ -173,6 +189,17 @@ def writeDataset(filename, vertices, edges, mode, outliers=0, switchPrior=1, swi
else:
isOdometryEdge = True

# auto determine information matrix for additional outliers from the first loop closure edge we find in the dataset
if not isOdometryEdge and informationMatrix==None:
elem = oldStr.split()
if mode==2:
informationMatrix = ' '.join(elem[-6:])
else:
informationMatrix = ' '.join(elem[-21:])
print informationMatrix


# carry on adding edges
if doSwitchable and not isOdometryEdge:
s=' '.join(['VERTEX_SWITCH', str(poseCount + switchCount), str(switchPrior)])
f.write(s+'\n')
Expand All @@ -187,9 +214,34 @@ def writeDataset(filename, vertices, edges, mode, outliers=0, switchPrior=1, swi
switchCount = switchCount + 1
elif doMaxMix and not isOdometryEdge:
elem = oldStr.split()
s = ' '.join([elem[0]+'_MAXMIX'] + [elem[1], elem[2], str(maxmixWeight)] + elem[3:])
s = ' '.join([edgeStr+'_MAXMIX'] + [elem[1], elem[2], str(maxmixScale)] + elem[3:])
f.write(s+'\n')

elif doMaxMixAgarwal and not isOdometryEdge:
elem = oldStr.split()

# first component edge
edge1 = ' '.join([edgeStr, "1"] + elem[1:])


# length of information matrix entry
if mode==2:
nInfo = 6
else:
nInfo=21

# build the weighted information matrix for the second component
info_str = elem[-nInfo:]
w = float(maxmixScale)
weighted_info_str = [str(float(x)*w) for x in info_str]

# second component edge
edge2 = ' '.join([edgeStr, str(maxmixWeight)]+ elem[1:-nInfo] + weighted_info_str)

# put it together
s = ' '.join([edgeStr+'_MIXTURE'] + [elem[1], elem[2], "2", edge1, edge2])
f.write(s+'\n')

else:

f.write(oldStr)
Expand All @@ -198,7 +250,7 @@ def writeDataset(filename, vertices, edges, mode, outliers=0, switchPrior=1, swi

# now create the desired number of additional outlier edges
for i in range(outliers):

elem = oldStr.split()

# determine random indices for the two vertices that are connected by an outlier edge
Expand Down Expand Up @@ -236,6 +288,11 @@ def writeDataset(filename, vertices, edges, mode, outliers=0, switchPrior=1, swi
yaw = random.gauss(0,sigma)
(q0, q1, q2, q3) = euler_to_quat(yaw, pitch, roll)

if perfectMatch:
x1=x2=x3=0
q0=1
q1=q2=q3=0


for j in range(groupSize):

Expand All @@ -260,12 +317,27 @@ def writeDataset(filename, vertices, edges, mode, outliers=0, switchPrior=1, swi
f.write(s+'\n')
switchCount = switchCount + 1
elif doMaxMix:
n=[v1, v2, maxmixWeight, x1, x2, x3]
n=[v1, v2, maxmixScale, x1, x2, x3]
if mode == 3:
n.extend([q0, q1, q2, q3])

s = ' '.join([edgeStr+'_MAXMIX'] + [str(x) for x in n]) + " " + info_str
f.write(s+'\n')
elif doMaxMixAgarwal:
n= v1, v2, x1, x2, x3

# edge component 1
edge1 = ' '.join([edgeStr, "1"] + [str(x) for x in n]) + " " + info_str

# edge component 2
w = float(maxmixScale)
weighted_info_str = [str(float(x)*w) for x in info_str.split()]
edge2 = ' '.join([edgeStr, str(maxmixWeight)] + [str(x) for x in n] + weighted_info_str)

# put it together
s = ' '.join([edgeStr+'_MIXTURE', str(v1), str(v2), "2"]) + " " + edge1 + " " + edge2
f.write(s+'\n')

else:
n=[v1, v2, x1, x2, x3]
if mode == 3:
Expand Down Expand Up @@ -297,16 +369,18 @@ def writeDataset(filename, vertices, edges, mode, outliers=0, switchPrior=1, swi
parser.add_option("-n", "--outliers", help = "Spoil the dataset with this many outliers. Default = 100.", default=100, type="int")
parser.add_option("-g", "--groupsize", help = "Use this groupsize. Default = 1.", default=1, type="int")
parser.add_option("--switchCov", help = "Set the switch covariance matrix. Default = 1.0", default=1.0, type="float")
parser.add_option("--information", help = "Set the information matrix for the additional false positive loop closure constraints. Provide either a single value e.g. --information=42 that will be used for all diagonal entries. Or provide the full upper triangular matrix using values separated by commas, but no spaces: --information=42,0,0,42,0,42 etc.", default="42.7,0,0,42.7,0,42.7")
parser.add_option("--information", help = "Set the information matrix for the additional false positive loop closure constraints. Provide either a single value e.g. --information=42 that will be used for all diagonal entries. Or provide the full upper triangular matrix using values separated by commas, but no spaces: --information=42,0,0,42,0,42 etc.") #, default="42.7,0,0,42.7,0,42.7")
parser.add_option("--seed", help = "Random seed. If >0 it will be used to initialize the random number generator to create repeatable random false positive loop closures.", default=None, type="int")
parser.add_option("--maxmixWeight", help = "Weight factor for the null hypothesis used in the max-mixture model. Default = 10e-8.", default=10e-8, type="float")

parser.add_option("--maxmixWeight", help = "Weight factor for the null hypothesis used in the max-mixture model. Default = 0.01", default=0.01, type="float")
parser.add_option("--maxmixScale", help = "Scale factor for the null hypothesis used in the max-mixture model. Default = 10e-12", default=10e-12, type="float")


# boolean options
parser.add_option("-s", "--switchable", help = "Use the switchable loop closure constraints.", action="store_true", default=False)
parser.add_option("-m", "--maxmix", help = "Use the max-mixture loop closure constraints.", action="store_true", default=False)
parser.add_option("-m", "--maxmix", help = "Use the max-mixture loop closure constraints.", action="store_true", default=False)
parser.add_option("--maxmixAgarwal", help = "Use the max-mixture loop closure constraints but create a dataset file that is compatible to the format of Pratik Agarwal's original Max-Mixture code.", action="store_true", default=False)
parser.add_option("-l", "--local", help = "Create only local false positive loop closure constraints.", action="store_true", default=False)

parser.add_option("-p", "--perfectMatch", help = "Loop closures match perfectly, i.e. the transformation between both poses is (0,0,0).", action="store_true", default=False)


# parse the command line options
Expand All @@ -330,11 +404,14 @@ def writeDataset(filename, vertices, edges, mode, outliers=0, switchPrior=1, swi
1,
options.switchCov,
options.maxmixWeight,
options.maxmixScale,
options.groupsize,
options.local,
options.information,
options.switchable,
options.maxmix):
options.maxmix,
options.maxmixAgarwal,
options.perfectMatch):
print "Done."


Expand Down
Loading

0 comments on commit f5de381

Please sign in to comment.