if opts.plot_axes_limit is not None:
ax.axis(opts.plot_axes_limit)
# ax.axis([-200, 100, -100, 200])
ax.set_aspect('equal','datalim')
ax.scatter ([x[0] for x in point_t],[x[1] for x in point_t], color='green', s=.3)
# graphVertices = getVertexFromGraph(f_handle_processed_graph, 1700, True)
# test_poses = getFloatNumberFromReadLines(open('/home/avavav/avdata/alphard/medialink/20150918-180619/icp_lm_poses.txt','r'), 13)
# test_poses = getFloatNumberFromReadLines(open('/home/avavav/avdata/alphard/medialink/20150918-174721/icp_lm_poses.txt','r'), 13)
# test_poses = getFloatNumberFromReadLines(open('/home/avavav/avdata/alphard/medialink/20150918-175207/icp_lm_poses.txt','r'), 13)
# test_poses = getFloatNumberFromReadLines(open(os.path.join(dir_prefix , 'icp_lm_poses.txt'),'r'), 13)
test_poses = IcpTestTools.getFloatNumberListFromReadLines(open(os.path.join(dir_prefix , 'icp_lm_poses.txt'),'r'))
# n = 1
vertices_plot = []
odom_plot = []
only_odom_accum_plot = []
icp_plot = []
# initial_error = [5,5,math.radians(10)]
# initial_error = [-30,0,0] #-15
initial_error = [0,0,0] #-15
if opts.initial_pose is not None:
init_pose_x = opts.initial_pose[0]
init_pose_y = opts.initial_pose[1]
init_pose_yaw = math.radians(opts.initial_pose[2])
else:
init_pose_x = 0
init_pose_y = 0
def getCloudFromFile(filename):
return IcpTestTools.getFloatNumberFromReadLines(open(filename, "r"), 3)
