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()