def main():
# Reads from the data file and runs estimate for each row
# Then plots the trajectory
data_array = util.read_csv(config.DATASET_ABSOLUTE_PATH)
row = data_array[0]
time, encoder, angular_velocity, steering_angle = np.ravel(row)
resulting_pos_heading = []
pose_estimator = PoseEstimator((0, 0, 0), time, encoder, angular_velocity,
steering_angle)
i = 1
while i < len(data_array):
row = data_array[i]
time, encoder, angular_velocity, steering_angle = np.ravel(row)
x, y, heading = pose_estimator.estimate(
time=time,
steering_angle=steering_angle,
encoder_ticks=encoder,
angular_velocity=angular_velocity)
resulting_pos_heading.append([x, y, heading])
i = i + 1
visualizer.plot_points(
np.asarray(resulting_pos_heading)[:, 0],
np.asarray(resulting_pos_heading)[:, 1])
visualizer.show()
def test_turn_location_position(self):
# Human assisted test
# Ensure that the car returns to the same location if we drive around in circles.
import visualizer
pose_estimator = PoseEstimator((0, 0, 0), 0, 0, 0, 0)
d = 4
steering_angle_radians = np.pi / d
outer_turn_radius_meters = pose_estimator.calc_outer_turn_radius(
steering_angle_radians)
front_wheel_turn_circumference = 2 * np.pi * config.FRONT_WHEEL_RADIUS_METERS
turn_circle_circumference = 2 * np.pi * outer_turn_radius_meters
ticks_required = config.ENCODER_RESOLUTION_FRONT_WHEEL * turn_circle_circumference / front_wheel_turn_circumference
result_loc = []
ticks = 0
for i in range(2 * d):
result_loc.append(
pose_estimator.estimate(time=i,
steering_angle=steering_angle_radians,
encoder_ticks=ticks,
angular_velocity=0))
ticks = ticks + ticks_required / (2 * d)
for loc in result_loc:
visualizer.draw_car(loc[0], loc[1], loc[2])
visualizer.show()
def test_outer_radius(self):
pose_estimator = PoseEstimator((0, 0, 0), 0, 0, 0, 0)
steering_angle_radians = np.pi / 2
self.assertEqual(
config.DISTANCE_FROM_FRONT_WHEEL_BACK_AXIS,
pose_estimator.calc_outer_turn_radius(steering_angle_radians))
steering_angle_radians = np.pi / 4
self.assertEqual(
config.DISTANCE_FROM_FRONT_WHEEL_BACK_AXIS /
np.sin(steering_angle_radians),
pose_estimator.calc_outer_turn_radius(steering_angle_radians))
steering_angle_radians = 0
self.assertEqual(
np.inf,
pose_estimator.calc_outer_turn_radius(steering_angle_radians))
import json
import numpy as np
import os
from estimator import PoseEstimator
if __name__ == '__main__':
pose_estimator = PoseEstimator()
filenames = [f.path for f in os.scandir('./images') if f.is_file()]
print('Generating dataset for', len(filenames), 'images')
dataset = []
for f in filenames:
print(f)
result = pose_estimator.get_image_file_result(f)
pose = f.split('./images/')[1].split('_')[0]
data = {'pose': f.split('./images/')[1].split('_')[0]}
data['left_elbow_angle'], data[
'right_elbow_angle'] = PoseEstimator.get_elbow_angles(result,
scaled=True)
data['left_wrist_right_elbow_distance'], data['right_wrist_left_elbow_distance'] = \
PoseEstimator.get_wrist_to_other_elbow_distances(result, scaled=True)
data['ankle_distance'] = PoseEstimator.get_ankle_distance(result,
scaled=True)
def test_straight_location_estimation(self):
# Testing the position for straight line motion
pose_estimator = PoseEstimator((0, 0, 0), 0, 0, 0, 0)
pose_estimator.estimate(0, 0, 0, 0)
self.assertEqual((2 * np.pi * config.FRONT_WHEEL_RADIUS_METERS, 0, 0),
pose_estimator.estimate(1, 0, 512, 10))