import matplotlib.pyplot as plt from Visualization import Plot3D import numpy as np id = np.identity(3) fig = plt.figure() ax = fig.add_subplot(111, projection='3d') #Plot3D.plot_wireframe_ellipsoid(1,1,1,id,ax,label_axes=True, clear=True,draw=False) Plot3D.plot_wireframe_ellipsoid(2,0.1,1, id,ax,label_axes=True, clear=False,draw=True)
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) w,v = Utils.covariance_eigen_decomp(motion_cov_small) z_factor, x_factor = Ackermann.get_standard_deviation_factors_for_projection(w) change_of_basis = np.identity(4,dtype=Utils.matrix_data_type) #change_of_basis = SE3.rotation_around_x(-math.pi/2) # testing inverse cov_inv = np.linalg.inv(motion_cov_small) t = np.matmul(cov_inv,motion_cov_small) fig = plt.figure() ax = fig.add_subplot(111, projection='3d') #Plot3D.plot_wireframe_ellipsoid(1,1,1,ax,label_axes=True, clear=True,draw=False) Plot3D.plot_wireframe_ellipsoid(0.1, [(x_factor,z_factor)], [change_of_basis], ax, label_axes=True, clear=False,draw=True)
motion_delta = ackermann_motion.pose_delta_list[0]
se3 = SE3.generate_se3_from_motion_delta(motion_delta)
origin_transformed = np.matmul(se3, origin)
origin_transformed_2 = np.matmul(se3, origin_transformed)
points = np.append(np.append(origin, origin_transformed, axis=1),
origin_transformed_2,
axis=1)
points_xyz = points[0:3, :]
# testing inverse
#cov_inv = np.linalg.inv(motion_cov_small_1)
#t = np.matmul(cov_inv,motion_cov_small_1)
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
#Plot3D.plot_wireframe_ellipsoid(1,1,1,ax,label_axes=True, clear=True,draw=False)
Plot3D.plot_array_lines(points_xyz, ax, clear=True, draw=False)
Plot3D.plot_wireframe_ellipsoid(0.1,
ellipse_factor_list,
se3_list,
ax,
label_axes=True,
clear=False,
draw=False)
ax.set_xlim(-10, 10)
ax.set_ylim(-1, 1)
ax.set_zlim(-10, 10)
Plot3D.show()