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