from MotionModels import Ackermann, SteeringCommand, MotionDeltaRobot, Pose import numpy as np from Numerics import Utils, SE3 import matplotlib.pyplot as plt from Visualization import Plot3D X, Y, Z = [0,0], [0,0], [0,-1] H = np.repeat(1,2) origin = Utils.to_homogeneous_positions(X, Y, Z, H) dt = 1.0 pose = Pose.Pose() steering_command_straight = SteeringCommand.SteeringCommands(1.0, 0.0) ackermann_motion = Ackermann.Ackermann() new_motion_delta = ackermann_motion.ackermann_dead_reckoning_delta(steering_command_straight) pose.apply_motion(new_motion_delta,dt) #TODO investigate which theta to use # this might actually be better since we are interested in the uncertainty only in this timestep theta = new_motion_delta.delta_theta # traditional uses accumulated theta #theta = pose.theta motion_cov_small = ackermann_motion.covariance_dead_reckoning(steering_command_straight,theta,dt) se3 = SE3.generate_se3_from_motion_delta(new_motion_delta) origin_transformed = np.matmul(se3, origin)
def __init__(self,
ground_truth_list=None,
plot_steering=False,
title=None,
plot_trajectory=True,
plot_rmse=True):
self.ground_truth_list = []
if ground_truth_list is not None:
self.ground_truth_list = ground_truth_list
self.figure = plt.figure()
self.plot_steering = plot_steering
self.plot_trajectory = plot_trajectory
self.plot_rmse = plot_rmse
if title:
plt.title(title)
if not plot_trajectory and not plot_rmse:
if not self.plot_steering:
self.x_graph = self.figure.add_subplot(131)
self.y_graph = self.figure.add_subplot(132)
self.z_graph = self.figure.add_subplot(133)
else:
self.x_graph = self.figure.add_subplot(231)
self.y_graph = self.figure.add_subplot(232)
self.z_graph = self.figure.add_subplot(233)
self.rev = self.figure.add_subplot(223)
self.steer = self.figure.add_subplot(224)
elif not plot_steering:
if plot_trajectory:
self.se3_graph = self.figure.add_subplot(311, projection='3d')
self.x_graph = self.figure.add_subplot(334)
self.y_graph = self.figure.add_subplot(335)
self.z_graph = self.figure.add_subplot(336)
if self.plot_rmse:
self.rmse_graph = self.figure.add_subplot(313)
else:
if plot_trajectory:
self.se3_graph = self.figure.add_subplot(411, projection='3d')
self.x_graph = self.figure.add_subplot(434)
self.y_graph = self.figure.add_subplot(435)
self.z_graph = self.figure.add_subplot(436)
if self.plot_rmse:
self.rmse_graph = self.figure.add_subplot(413)
self.rev = self.figure.add_subplot(427)
self.steer = self.figure.add_subplot(428)
if plot_steering:
self.rev.set_title("rev cmd")
self.steer.set_title("str cmd")
self.rev.set_xlabel("frame")
self.steer.set_xlabel("frame")
self.rev.set_ylabel("input level")
self.steer.set_ylabel("input level")
#self.se3_graph.set_aspect('equal')
if self.plot_trajectory:
self.se3_graph.set_title("relative pose estimate")
self.x_graph.set_title("X")
self.x_graph.set_title("X")
self.y_graph.set_title("Y")
self.z_graph.set_title("Z")
self.x_graph.set_xlabel("frame")
self.y_graph.set_xlabel("frame")
self.z_graph.set_xlabel("frame")
self.x_graph.set_ylabel("meters")
if self.plot_rmse:
self.rmse_graph.set_title("drift per frame (pose error)")
# These points will be plotted with incomming se3 matricies
X, Y, Z = [0, 0], [0, 0], [0, -1]
#X, Y, Z = [0, 0], [0, 0], [0, 1]
H = np.repeat(1, 2)
self.point_pair = Utils.to_homogeneous_positions(X, Y, Z, H)
if self.plot_steering:
plt.subplots_adjust(left=0.07)
else:
plt.subplots_adjust(left=0.05)
import matplotlib.pyplot as plt from Visualization import Plot3D from Numerics import Generator, Utils import numpy as np import math #X, Y, Z = [0,0,10,10] , [0,0,10,10] , [0,-1,0,-1] X, Y, Z = [0, 0], [0, 0], [0, -1] H = np.repeat(1, 2) pair = Utils.to_homogeneous_positions(X, Y, Z, H) se3 = Generator.generate_random_se3(2, 2, math.pi, math.pi / 2, 0, 0) se3_2 = Generator.generate_random_se3(2, 2, math.pi, math.pi / 2, 0, 0) pair_transformed = np.matmul(se3, pair) pair_transformed_2 = np.matmul(se3_2, pair) points = np.append(pair, pair_transformed, axis=1) points_xyz = points[0:3, :] fig = plt.figure() ax = fig.add_subplot(111, projection='3d') Plot3D.plot_array_lines(points_xyz, ax) pair_transformed = np.matmul(se3, pair)