예제 #1
0
    def loadData (self):
        if (self.orbResultChooser.GetValue()=='' or self.groundTruthChooser.GetValue()==''):
            print ("Select filenames first")
            return None
        print ("Wait...")
        
        # Load ground truth and plot it
        self.ax = self.figure.add_subplot(111)
        self.ax.set_autoscale_on(True)
        self.ax.grid(True)
        self.groundTruth = PoseTable.loadFromBagFile(self.groundTruthChooser.GetValue(), 'world', 'ndt_frame')
        gtTbl = self.groundTruth.toArray()
        self.groundTruthPlot, = self.ax.plot(gtTbl[:,0], gtTbl[:,1])
#        self.groundTruthPlot, = self.ax.plot([0,1,2], [4,5,6])
        
        self.orbResult = PoseTable.loadFromBagFile(self.orbResultChooser.GetValue(), '/ORB_SLAM/World', '/ORB_SLAM/ExtCamera')
        orbTbl = self.orbResult.toArray()
        self.orbResultPlot, = self.ax.plot(orbTbl[:,0], orbTbl[:,1])
        
        self.canvas.draw()
        self.background = self.canvas.copy_from_bbox(self.ax.bbox)
#        self.helpText.Show(True)
        
        self.timeChooser.SetRange(self.orbResult[0].timestamp, self.orbResult.last().timestamp)
        self.currentORBTimestamp = self.orbResult[0].timestamp
        self.redrawPosition()
예제 #2
0
    def loadData(self):
        if (self.orbResultChooser.GetValue() == ''
                or self.groundTruthChooser.GetValue() == ''):
            print("Select filenames first")
            return None
        print("Wait...")

        # Load ground truth and plot it
        self.ax = self.figure.add_subplot(111)
        self.ax.set_autoscale_on(True)
        self.ax.grid(True)
        self.groundTruth = PoseTable.loadFromBagFile(
            self.groundTruthChooser.GetValue(), 'world', 'ndt_frame')
        gtTbl = self.groundTruth.toArray()
        self.groundTruthPlot, = self.ax.plot(gtTbl[:, 0], gtTbl[:, 1])
        #        self.groundTruthPlot, = self.ax.plot([0,1,2], [4,5,6])

        self.orbResult = PoseTable.loadFromBagFile(
            self.orbResultChooser.GetValue(), '/ORB_SLAM/World',
            '/ORB_SLAM/ExtCamera')
        orbTbl = self.orbResult.toArray()
        self.orbResultPlot, = self.ax.plot(orbTbl[:, 0], orbTbl[:, 1])

        self.canvas.draw()
        self.background = self.canvas.copy_from_bbox(self.ax.bbox)
        #        self.helpText.Show(True)

        self.timeChooser.SetRange(self.orbResult[0].timestamp,
                                  self.orbResult.last().timestamp)
        self.currentORBTimestamp = self.orbResult[0].timestamp
        self.redrawPosition()
예제 #3
0
        self.frame.Show(True)
        self.SetTopWindow(self.frame)
        return True


class tfBagVisualizer (wx.Frame):
    
    def __init__ (self, bag):
        pass
    
    
if __name__ == '__main__':
    
    from sys import argv
    
    groundTruth = PoseTable.loadFromBagFile (argv[1], 'world', 'camera1')
    app = wx.PySimpleApp()
    frame = PlotFigure(groundTruth)
    timer = wx.Timer(frame, TIMER_ID)
    timer.Start(50)
    
    rospy.init_node('ORB_Listener', anonymous=True)
    orbListener = tf.TransformListener()
    
    orbProc1 = TfCollector ('world', 'camera1', orbListener)
    orbProc2 = TfCollector ('world', 'camera2', orbListener)
    
    frame.Show()
    app.MainLoop()
    
    orbProc1.stop = True
        self.frame = AnalyzerWindow()
        self.frame.Show(True)
        self.SetTopWindow(self.frame)
        return True


class tfBagVisualizer(wx.Frame):
    def __init__(self, bag):
        pass


if __name__ == '__main__':

    from sys import argv

    groundTruth = PoseTable.loadFromBagFile(argv[1], 'world', 'camera1')
    app = wx.PySimpleApp()
    frame = PlotFigure(groundTruth)
    timer = wx.Timer(frame, TIMER_ID)
    timer.Start(50)

    rospy.init_node('ORB_Listener', anonymous=True)
    orbListener = tf.TransformListener()

    orbProc1 = TfCollector('world', 'camera1', orbListener)
    orbProc2 = TfCollector('world', 'camera2', orbListener)

    frame.Show()
    app.MainLoop()

    orbProc1.stop = True
예제 #5
0

if __name__ == '__main__':

    # Map loading
    _map1m = np.loadtxt(
        '/home/sujiwo/Data/Road_Datasets/2016-01-21-15-12-49/s1-map.csv')
    _map2m = np.loadtxt(
        '/home/sujiwo/Data/Road_Datasets/2016-01-21-15-12-49/s2-map.csv')

    # For ground truth
    tmp1g = np.zeros((_map1m.shape[0], 8))
    tmp2g = np.zeros((_map2m.shape[0], 8))
    tmp1g[:, 0] = _map1m[:, 0]
    tmp1g[:, 1:] = _map1m[:, 8:]
    tmp2g[:, 0] = _map2m[:, 0]
    tmp2g[:, 1:] = _map2m[:, 8:]
    mapGroundTruth1 = PoseTable.loadFromArray(tmp1g)
    mapGroundTruth2 = PoseTable.loadFromArray(tmp2g)

    # For ORB Poses
    tmp1o = np.zeros(tmp1g.shape)
    tmp2o = np.zeros(tmp1g.shape)
    tmp1o[:, 0] = _map1m[:, 0]
    tmp1o[:, 1:] = _map1m[:, 1:8]
    tmp2g[:, 0] = _map2m[:, 0]
    tmp2g[:, 1:] = _map2m[:, 1:8]
    mapOrb1 = PoseTable.loadFromArray(tmp1o)
    mapOrb2 = PoseTable.loadFromArray(tmp2o)

    print('XXX')
예제 #6
0
    pass


if __name__ == "__main__":

    # Map loading
    _map1m = np.loadtxt("/home/sujiwo/Data/Road_Datasets/2016-01-21-15-12-49/s1-map.csv")
    _map2m = np.loadtxt("/home/sujiwo/Data/Road_Datasets/2016-01-21-15-12-49/s2-map.csv")

    # For ground truth
    tmp1g = np.zeros((_map1m.shape[0], 8))
    tmp2g = np.zeros((_map2m.shape[0], 8))
    tmp1g[:, 0] = _map1m[:, 0]
    tmp1g[:, 1:] = _map1m[:, 8:]
    tmp2g[:, 0] = _map2m[:, 0]
    tmp2g[:, 1:] = _map2m[:, 8:]
    mapGroundTruth1 = PoseTable.loadFromArray(tmp1g)
    mapGroundTruth2 = PoseTable.loadFromArray(tmp2g)

    # For ORB Poses
    tmp1o = np.zeros(tmp1g.shape)
    tmp2o = np.zeros(tmp1g.shape)
    tmp1o[:, 0] = _map1m[:, 0]
    tmp1o[:, 1:] = _map1m[:, 1:8]
    tmp2g[:, 0] = _map2m[:, 0]
    tmp2g[:, 1:] = _map2m[:, 1:8]
    mapOrb1 = PoseTable.loadFromArray(tmp1o)
    mapOrb2 = PoseTable.loadFromArray(tmp2o)

    print("XXX")