def track_marker(select_files=False, webcam_stream=False):
save_figs = False # Save the resulting plots
# Define marker parameters
dictionary = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50)
board = cv2.aruco.CharucoBoard_create(3, 3, .07, .035, dictionary)
marker_size = 0.10 # In meter
currentdir = os.path.dirname(
os.path.abspath(inspect.getfile(inspect.currentframe())))
parentdir = os.path.dirname(currentdir)
sys.path.insert(0, parentdir)
# Select files with GUI or use example data
if select_files:
cam_params_file = get_file_path("Select camera parameters file").name
video_file = get_file_path("Select video file").name
else:
cam_params_file = str(sys.path[0]) + '/example_data/cam_params.pckl'
video_file = str(sys.path[0]) + '/example_data/robot_aruco.webm'
all_tvec, all_rvec = webcam.get_webcam_reference(
video_file,
cam_params_file,
dictionary,
marker_size,
board,
show_video=True,
save_output=False,
output_file_name='example.avi',
webcam_stream=webcam_stream)
# Project to single plane
(X, Y, Z) = project_in_main_plane(all_tvec)
# fig = plt.figure(3)
# fig.clf()
# ax = fig.gca(projection='3d')
# ax.axis('equal')
# ax.scatter(X, Y)
visualize(X, Y, Z, saveFig=save_figs)
def track_marker():
save_figs = False # Save the resulting plots
# Define marker parameters
dictionary = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50)
board = cv2.aruco.CharucoBoard_create(3, 3, .07, .035, dictionary)
marker_size = 0.10 # In meter
currentdir = os.path.dirname(
os.path.abspath(inspect.getfile(inspect.currentframe())))
parentdir = os.path.dirname(currentdir)
sys.path.insert(0, parentdir)
# Select files with GUI
cam_params_file = get_file_path("Select camera parameters file").name
video_file = get_file_path("Select video file").name
# cam_params_file = '/home/quinten/Documents/vakantiejob/camera/benchmarking/example_data/cam_params.pckl'
# video_file = '/home/quinten/Documents/vakantiejob/camera/benchmarking/example_data/robot_aruco.webm'
all_tvec, all_rvec = get_webcam_reference(video_file,
cam_params_file,
dictionary,
marker_size,
board,
show_video=True,
save_output=False,
output_file_name='example.avi')
print(all_tvec.shape)
(X, Y, Z) = project_in_main_plane(all_tvec)
fig = plt.figure(3)
fig.clf()
ax = fig.gca(projection='3d')
ax.axis('equal')
ax.scatter(X, Y)
visualize(all_tvec, saveFig=save_figs)
write_to_csv(np.column_stack((X, Y, Z)), '/tmp/test_vid.csv', 0, 30)
if __name__ == "__main__":
args = sys.argv
topics = [
'/light_intensity', '/mobile_base/sensors/core',
'/mobile_base/sensors/imu_data', '/mobile_base/sensors/imu_data_raw',
'/visualization_marker', '/hedge_1/beacons_pos_a',
'/hedge_1/hedge_pos', '/hedge_1/hedge_pos_a', '/hedge_1/hedge_pos_ang',
'/hedge_2/beacons_pos_a', '/hedge_2/hedge_pos', '/hedge_2/hedge_pos_a',
'/hedge_2/hedge_pos_ang'
]
# If no argument is passed, filter a single file
if len(sys.argv) < 2:
bagFile = get_file_path('Select bag file').name
filter_bag_file(bagFile, topics)
# If a valid argument is passed, filter the entire directory and all subdirectories
else:
filter_arg = sys.argv[1]
acceptable_agrs = [
'dir', 'Dir', 'directory', 'Directory', 'filterdir',
'filterdirectory', 'FilterDirectory', 'filter_dir',
'filter_directory'
]
if filter_arg in acceptable_agrs:
bag_dir = get_directory_path(
'Select directory of .bag files to filter (SUBDIRECTORIES WILL BE FILTERED TOO)'
) # Select directory via GUI
I_Tx = 1.05 # Driving current [A]
# Plotting parameters
plot_interval = 2
dpi = 300
figNum = 1
titleSize = 14
legendSize = 12
axesSize = 12
labelSize = 12
dpi = 1000
legendLoc = 3
#%% GET DATA FROM FILES
# Select files with GUI
bagFilePath = get_file_path("Select .bag file").name
# Get robot position estimate
pose_ref, t_ref = get_joint_data(bagFilePath, 'base_footprint')
pose_est, t_est = get_joint_data(bagFilePath, 'pose')
x_est = pose_est[0]
y_est = pose_est[1]
x_ref = pose_ref[0]
y_ref = pose_ref[1]
#%% CALCULATE POSITIONING ERROR
dr = get_dr(x_est, y_est, x_ref, y_ref)
#%% PLOTTING
from mpl_toolkits.mplot3d import Axes3D
from file_select_gui import get_file_path
from webcam import get_webcam_reference
fontSize = 20 # Size of labels for plots
save_figs = False # Save the resulting plots
plot_interval = 1.0 #
# Define marker parameters
dictionary = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50)
board = cv2.aruco.CharucoBoard_create(3, 3, .07, .035, dictionary)
marker_size = 0.10 # In meter
# Select files with GUI
cam_params_file = get_file_path("Select camera parameters file").name
video_file = get_file_path("Select video file").name
all_tvec, all_rvec = get_webcam_reference(video_file,
cam_params_file,
dictionary,
marker_size,
board,
show_video=True,
save_output=False,
output_file_name='example.avi')
# PLOTTING
fig_name = 'Robot position'
fig1 = plt.figure(1)
fig1.clf()