def testUpdate(self):
# Create a uniform random field
g = Guesser(
"map_filter_test", self.bg.nPoints, (0.0, 0.0, 2 * math.pi), "location", HistogramData((10, 10, 1))
)
g.uniform([[-5.0, 5.0], [-5.0, 5.0], [0.0, 3.0]])
def testUpdate(self):
# Create a uniform random field
g = Guesser("map_filter_test", (0.0, 0.0, 2*math.pi),
"location", (10,10,1))
g.uniform(self.mg.nPoints, [[-5.0,5.0],[-5.0,5.0],[0.0,3.0]])
# Use it as a guess against the filter already in mg
gr = GuessRequest(inPoints=g.outPoints(),
means=g.means(),
stds=g.stds(),
data_type=g.data_type,
pers=g.periods)
gresp = self.mg.handle_guess(gr)
# There should be no points left where the mapData array was 1.0
xmax = False ; ymin = False ; xyint = False
xvec = [ p.point[0] for p in gresp.outPoints ]
yvec = [ p.point[1] for p in gresp.outPoints ]
print ">>>", max(xvec), min(xvec)
print ">>>", max(yvec), min(yvec)
mx = map(max, self.mg.guesser.pointArray.transpose())
print ">>>>", mx
self.assertTrue(mx[0] < 2.5 and mx[1] < 2.5)
mn = map(min, self.mg.guesser.pointArray.transpose())
self.assertTrue(mn[0] > -3.5 and mn[1] > -3.5)
cornerTest = False
npts = self.mg.guesser.pointArray.shape[0]
for i in range(npts):
if self.mg.guesser.pointArray[i][0] > 0.25 and \
self.mg.guesser.pointArray[i][1] < -0.5:
cornerTest = True
self.assertFalse(cornerTest)
def testUpdate(self):
# Create a uniform random field
g = Guesser("map_filter_test", (0.0, 0.0, 2 * math.pi), "location",
(10, 10, 1))
g.uniform(self.mg.nPoints, [[-5.0, 5.0], [-5.0, 5.0], [0.0, 3.0]])
# Use it as a guess against the filter already in mg
gr = GuessRequest(inPoints=g.outPoints(),
means=g.means(),
stds=g.stds(),
data_type=g.data_type,
pers=g.periods)
gresp = self.mg.handle_guess(gr)
# There should be no points left where the mapData array was 1.0
xmax = False
ymin = False
xyint = False
xvec = [p.point[0] for p in gresp.outPoints]
yvec = [p.point[1] for p in gresp.outPoints]
print ">>>", max(xvec), min(xvec)
print ">>>", max(yvec), min(yvec)
mx = map(max, self.mg.guesser.pointArray.transpose())
print ">>>>", mx
self.assertTrue(mx[0] < 2.5 and mx[1] < 2.5)
mn = map(min, self.mg.guesser.pointArray.transpose())
self.assertTrue(mn[0] > -3.5 and mn[1] > -3.5)
cornerTest = False
npts = self.mg.guesser.pointArray.shape[0]
for i in range(npts):
if self.mg.guesser.pointArray[i][0] > 0.25 and \
self.mg.guesser.pointArray[i][1] < -0.5:
cornerTest = True
self.assertFalse(cornerTest)
def testUpdate(self):
# Create a uniform random field
g = Guesser("map_filter_test", self.bg.nPoints, (0.0, 0.0, 2*math.pi),
"location", HistogramData((10,10,1)))
g.uniform([[-5.0,5.0],[-5.0,5.0],[0.0,3.0]])
mapArray[xi,yi] = 100
if yi < 3 or yi > 15:
mapArray[xi,yi] = 100
if yi < 10 and xi > 10:
mapArray[xi,yi] = 75
mapArray.shape = 576
og.data = list(mapArray)
mapData = MapData(og)
bg.addMap(mapData)
import tgraph
tg = tgraph.Tgraph(350, 350)
g = Guesser("map_filter_test", bg.nPoints, (0.0, 0.0, 2*math.pi),
"location", HistogramData((10,10,1)))
g.uniform([[-5.0,5.0],[-5.0,5.0],[0.0,3.0]])
bg.handle_guess(g.outPoints())
plist = []
for pt in bg.mapPointList:
plist.append(pt.point)
tg.draw_scatter(np.array([[-5.0, -5.0, 0.0], [5.0, 5.0, 2.0]]), 0,1,2,"s")
tg.plot_points(np.array(plist), 0,1,2, "s")
tg.mainloop()
if xi < 3 or xi > 15:
mapArray[xi, yi] = 100
if yi < 3 or yi > 15:
mapArray[xi, yi] = 100
if yi < 10 and xi > 10:
mapArray[xi, yi] = 75
mapArray.shape = 576
og.data = list(mapArray)
mapData = MapData(og)
bg.addMap(mapData)
import tgraph
tg = tgraph.Tgraph(350, 350)
g = Guesser("map_filter_test", bg.nPoints, (0.0, 0.0, 2 * math.pi), "location", HistogramData((10, 10, 1)))
g.uniform([[-5.0, 5.0], [-5.0, 5.0], [0.0, 3.0]])
bg.handle_guess(g.outPoints())
plist = []
for pt in bg.mapPointList:
plist.append(pt.point)
tg.draw_scatter(np.array([[-5.0, -5.0, 0.0], [5.0, 5.0, 2.0]]), 0, 1, 2, "s")
tg.plot_points(np.array(plist), 0, 1, 2, "s")
tg.mainloop()
# print oms.formatOdometry(om[0])
oms.discardPrev(s)
ol = oms.fromTime(s)
# print oms.formatOdometry(ol[0])
o = OdomGuesser("testOdomGuesser", 1000, 0.1, 0.1, 0.1, debug=True)
import tgraph
from perfesser_guesser import *
g = Guesser("testGuesser", (0.0, 0.0, 2*math.pi),
"position", (4,4,7))
g.uniform(1000, [[-2.0,2.0], [-2.0,2.0], [0.0, 0.5]])
tg = tgraph.Tgraph(400,220)
corners = np.array([[-4.0, -4.0, -4.0], [4.0, 4.0, 4.0]])
tg.draw_scatter(corners, 0, 1, 2, "s")
tg.plot_points(g.pointArray, 0, 1, 2, "s")
o.updateOdom(o1)
o.updateOdom(o2)
o.updateOdom(o3)
o.updateOdom(o4)
s = o1.header.stamp
s.nsecs += 5000000
mg.addMap(mapData)
import tgraph
tg = tgraph.Tgraph(350, 200)
inarray = np.array([[-5.5, -5.5, 0.0], [5.5, 5.5, 0.6]])
tg.draw_scatter(inarray, 0, 1, 2, "s")
parray = np.array([p.point for p in mg.mapPointList])
tg.plot_points(parray, 0, 1, 2, "s")
g = Guesser("map_filter_test", (0.0, 0.0, 2 * math.pi), "location",
(10, 10, 1))
g.uniform(mg.nPoints, [[-5.0, 5.0], [-5.0, 5.0], [0.0, 3.0]])
tg.new_graph()
tg.draw_scatter(g.pointArray, 0, 1, 2, "s", recalc=False)
gr = GuessRequest(inPoints=g.outPoints(),
means=g.means(),
stds=g.stds(),
data_type=g.data_type,
pers=g.periods)
gresp = mg.handle_guess(gr)
tg.plot_points(mg.guesser.hist.plottable(), 0, 1, 2, "c")
tg.new_graph()
mg.addMap(mapData)
import tgraph
tg = tgraph.Tgraph(350,200)
inarray = np.array([[-5.5,-5.5,0.0],[5.5,5.5,0.6]])
tg.draw_scatter(inarray, 0,1,2, "s")
parray = np.array([p.point for p in mg.mapPointList])
tg.plot_points(parray, 0,1,2, "s")
g = Guesser("map_filter_test", (0.0, 0.0, 2*math.pi),
"location", (10,10,1))
g.uniform(mg.nPoints, [[-5.0,5.0],[-5.0,5.0],[0.0,3.0]])
tg.new_graph()
tg.draw_scatter(g.pointArray, 0,1,2, "s", recalc=False)
gr = GuessRequest(inPoints=g.outPoints(),
means=g.means(),
stds=g.stds(),
data_type=g.data_type,
pers=g.periods)
gresp = mg.handle_guess(gr)
tg.plot_points(mg.guesser.hist.plottable(), 0,1,2,"c")
tg.new_graph()