示例#1
0
        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]])
示例#2
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)
示例#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)
示例#4
0
        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]])
示例#5
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()

示例#6
0
                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()
示例#7
0
#    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
示例#8
0
        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()
示例#9
0
        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()