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)
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
greq = GuessRequest()
greq.source_stamp = s
greq.header.stamp = rospy.Time.now()
greq.pers = (0.0, 0.0, 2*math.pi)
greq.means = g.means()
greq.stds = g.stds()
greq.inPoints = g.outPoints()
gresp = o.updateBelief(greq)
pts = Belief(points = gresp.outPoints)
tg.new_graph()
pta = g.pointsToArray(pts)
print "maxs:", map(max, pta.transpose())
print "mins:", map(min, pta.transpose())
tg.draw_scatter(g.pointsToArray(pts), 0, 1, 2, "s", recalc=False)
tg.mainloop()
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()
parray = np.array([p.point for p in gresp.outPoints])
tg.draw_scatter(parray, 0, 1, 2, "s", recalc=False)
tg.mainloop()
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()
parray = np.array([p.point for p in gresp.outPoints])
tg.draw_scatter(parray, 0,1,2, "s", recalc=False)
tg.mainloop()