def exponential_map(r):
r"""
Apply the exponential map to a 3d vector representing an orientation;
:math:`exp : \mathbb{R}^3 \rightarrow \mathbb{S}^3`, where :math:`\mathbb{S}^3` is a unit sphere in
:math:`\mathbb{R}^4`.
The mapping is given by:
.. math::
\exp(\pmb{r}) = \left\{ \begin{array}{ll}
\cos(||\pmb{r}||) + \sin(||\pmb{r}||) \frac{r}{||r||}, & \pmb{r} \neq \pmb{0} \\
1, & \text{otherwise}
\end{array} \right.
where :math:`\pmb{r} \in \mathbb{R}^3`, and we use the following representation for the quaternion
:math:`q = s + \pmb{v}` where :math:`s \in \mathbb{R}` is its scalar part, and :math:`\pmb{v} \in \mathbb{R}^3` is
its vector part.
Args:
r (np.array[float[3]]): 3d vector
Returns:
np.array[float[4]]: quaternion
"""
if np.allclose(r, np.zeros(3)):
return quaternion.quaternion(1, 0, 0, 0)
r_ = np.linalg.norm(r)
x, y, z = np.sin(r_) * r / r_
return quaternion.quaternion(np.cos(r_), x, y, z)
def noise_application(noiseWidth, vector):
# Generate a random vector in solid angle 4*pi*nu around north pole
z = np.random.uniform(0., 1.) * (1 - cos(noiseWidth)) + cos(noiseWidth)
phi = np.random.uniform(0., 1.) * 2 * np.pi
x = sqrt(1 - z**2) * cos( phi )
y = sqrt(1 - z**2) * sin( phi )
# Rotate the noise vector to be in a cone around the directional vector
# rotation axis
# pole = np.array([0, 0, 1])
vector = vector/ sqrt(vector[0]**2 + vector[1]**2 + vector[2]**2)
u = np.cross([0, 0, 1], vector)
#u = u/norm(u)
# rotation angle
rotTheta = np.arccos(np.dot(vector, [0, 0, 1]))
#prepare rot angle for quaternion
axisAngle = 0.5*rotTheta * u / sqrt(u[0]**2 + u[1]**2 + u[2]**2)
# rotation matrix
#M = expm( np.cross( np.eye(3), u * rotTheta ) )
# Quaternion stuff - pretty fast, compared to other stuff...
vec = quat.quaternion(x, y, z)
qlog = quat.quaternion(*axisAngle)
q = np.exp(qlog)
vPrime = q * vec * np.conjugate(q)
return vPrime.imag
def __init__(
self,
transform: TransformType = None,
*,
translation: Iterable[float] = (0, 0, 0),
rotation: RotationType = (1, 0, 0, 0),
) -> None:
if transform is not None:
if isinstance(transform, Transform3D):
self._translation = transform.translation
self._rotation = transform.rotation
return
if isinstance(transform, Sequence): # pylint: disable=W1116
transform = np.array(transform)
if transform.shape != (3, 4) and transform.shape != (4, 4):
raise ValueError(
"The shape of input transform matrix must be 3x4 or 4x4.")
self._translation = Vector3D(transform[0, 3], transform[1, 3],
transform[2, 3])
self._rotation = from_rotation_matrix(transform)
return
self._translation = Vector3D(*translation)
if isinstance(rotation, quaternion):
self._rotation = quaternion(rotation)
else:
self._rotation = quaternion(*rotation)
def make_obs(self):
'''
State vector of length 17
contains two poses, and length,width and height of object bb
'''
state = None
if self.state_rep == "state":
objs = self.machine.get_objects(True)
gripper_pose = list(
self.machine.env._scene.get_observation().gripper_pose)
gripper_quat = gripper_pose[3:]
gripper_quat[0], gripper_quat[-1] = gripper_quat[-1], gripper_quat[
0]
quat = quaternion(*gripper_quat)
g_vec = list(as_rotation_vector(quat))
obj_pose = list(objs[OBJECT].get_pose())
obj_quat = obj_pose[3:]
obj_quat[0], obj_quat[-1] = obj_quat[-1], obj_quat[0]
quat = quaternion(*obj_quat)
r_vec = list(as_rotation_vector(quat))
obj_bb = objs[OBJECT].get_bounding_box()
x_diff = obj_bb[0] - obj_bb[3]
y_diff = obj_bb[1] - obj_bb[4]
z_diff = obj_bb[2] - obj_bb[5]
state = np.array(gripper_pose[:3]+g_vec+ \
obj_pose[:3]+r_vec+[x_diff,y_diff,z_diff]).reshape(1,-1)
elif self.state_rep == "vision":
obs = self.machine.env._scene.get_observation()
state = obs.wrist_rgb
# plt.imsave("image.png",state)
return state
def setUp(self):
self.rotation_a = quaternion.quaternion(-0.572, 0.198, 0.755, -0.252)
self.rotation_a_negative = quaternion.quaternion(
0.572, -0.198, -0.755, 0.252)
self.translation_a = [144.801, -74.548, -17.746]
self.pose_a = kapture.PoseTransform(self.rotation_a,
self.translation_a)
self.pose_a_negative = kapture.PoseTransform(self.rotation_a_negative,
self.translation_a)
self.rotation_b = quaternion.quaternion(0.878, 0.090, 0.374, -0.285)
self.rotation_b_negative = quaternion.quaternion(
-0.878, -0.090, -0.374, 0.285)
self.translation_b = [4.508, 45.032, -37.840]
self.pose_b = kapture.PoseTransform(self.rotation_b,
self.translation_b)
self.pose_b_negative = kapture.PoseTransform(self.rotation_b_negative,
self.translation_b)
self.pose_ab = kapture.PoseTransform(self.rotation_a,
self.translation_b)
self.pose_ba = kapture.PoseTransform(self.rotation_b,
self.translation_a)
self.pose_none = kapture.PoseTransform(r=None, t=None)
self.pose_r_none = kapture.PoseTransform(r=None, t=self.translation_a)
self.pose_t_none = kapture.PoseTransform(r=self.rotation_a, t=None)
def test_rmul(self):
translation = [1, 2, 3]
rotation = quaternion(0, 1, 0, 0)
transform = Transform3D(translation=translation, rotation=rotation)
quaternion_1 = quaternion(1, 2, 3, 4)
labeledbox3d = LabeledBox3D(transform,
category=_CATEGORY,
attributes=_ATTRIBUTES,
instance=_INSTANCE)
assert labeledbox3d.__rmul__(transform) == LabeledBox3D(
translation=[2, 0, 0],
rotation=[-1, 0, 0, 0],
category=_CATEGORY,
attributes=_ATTRIBUTES,
instance=_INSTANCE,
)
assert labeledbox3d.__rmul__(quaternion_1) == LabeledBox3D(
translation=[1.7999999999999996, 2, 2.6],
rotation=[-2, 1, 4, -3],
category=_CATEGORY,
attributes=_ATTRIBUTES,
instance=_INSTANCE,
)
assert labeledbox3d.__rmul__(1) == NotImplemented
def get_corrected_2_2(file_name):
h = scri.SpEC.read_from_h5(file_name + "/Extrapolated_N2.dir")
# Get the waveform in the coprecessing frame
i = quaternion.quaternion(0, 1, 0, 0)
j = quaternion.quaternion(0, 0, 1, 0)
k = quaternion.quaternion(0, 0, 0, 1)
def cross(a, b):
return 1 / 2 * (a * b - b * a)
k_hat = quaternion.quaternion(0, 0.25269634778, 0.04202266793,
0.80968828452)
k_hat = k_hat / np.sqrt(k_hat * np.conjugate(k_hat))
ax = cross(k, k_hat)
ax = ax / np.sqrt(ax * np.conjugate(ax))
cos_al = np.real(k * np.conjugate(k_hat))
alpha = np.arccos(cos_al)
rotor = np.concatenate(
[np.array([np.cos(0.5 * alpha)]),
np.sin(0.5 * alpha) * ax.imag])
h.to_coprecessing_frame()
#h_rot = h.transform(frame_rotation=rotor)
h_rot = h
return np.transpose(
np.stack([
h_rot.t,
np.real(h_rot.data[:, lm(2, 2, h_rot.ell_min)]),
np.imag(h_rot.data[:, lm(2, 2, h_rot.ell_min)])
]))
def test_init(self):
sequence = [[1, 0, 0, 1], [0, 1, 0, 1]]
with pytest.raises(ValueError):
Transform3D(sequence)
transform = Transform3D(None)
assert transform.translation == Vector3D(0.0, 0.0, 0.0)
assert transform.rotation == quaternion(1.0, 0.0, 0.0, 0.0)
sequence = [[1, 0, 0, 1], [0, 1, 0, 1], [0, 0, 1, 1]]
transform = Transform3D(sequence)
assert transform.translation == Vector3D(1, 1, 1)
assert transform.rotation == quaternion(1.0, -0.0, -0.0, -0.0)
numpy = np.array(sequence)
transform = Transform3D(numpy)
assert transform.translation == Vector3D(1, 1, 1)
assert transform.rotation == quaternion(1.0, -0.0, -0.0, -0.0)
transform_1 = Transform3D(transform)
assert transform_1.translation == Vector3D(1, 1, 1)
assert transform_1.rotation == quaternion(1.0, -0.0, -0.0, -0.0)
transform = Transform3D(translation=[1, 2, 3], rotation=[1, 0, 0, 0])
assert transform.translation == Vector3D(1, 2, 3)
assert transform.rotation == quaternion(1.0, 0.0, 0.0, 0.0)
def interpolate_quaternion_linear(quat_data, input_timestamp,
output_timestamp):
n_input = quat_data.shape[0]
assert input_timestamp.shape[0] == n_input
assert quat_data.shape[1] == 4
n_output = output_timestamp.shape[0]
quat_inter = np.zeros([n_output, 4])
ptr1 = 0
ptr2 = 0
for i in range(n_output):
if ptr1 >= n_input - 1 or ptr2 >= n_input:
raise ValueError("")
# Forward to the correct interval
while input_timestamp[ptr1 + 1] < output_timestamp[i]:
ptr1 += 1
if ptr1 == n_input - 1:
break
while input_timestamp[ptr2] < output_timestamp[i]:
ptr2 += 1
if ptr2 == n_input:
break
q1 = quaternion.quaternion(*quat_data[ptr1])
q2 = quaternion.quaternion(*quat_data[ptr2])
quat_inter[i] = quaternion.as_float_array(
quaternion.quaternion_time_series.slerp(q1, q2,
input_timestamp[ptr1],
input_timestamp[ptr2],
output_timestamp[i]))
return quat_inter
def test_rmul(self):
size = [1, 2, 3]
translation = [1, 2, 3]
rotation = quaternion(0, 1, 0, 0)
transform = Transform3D(translation, rotation)
quaternion_1 = quaternion(1, 2, 3, 4)
labeledbox3d = LabeledBox3D(
size=size,
translation=translation,
rotation=rotation,
category="cat",
attributes={"gender": "male"},
instance="12345",
)
assert labeledbox3d.__rmul__(transform) == LabeledBox3D(
size=size,
translation=[2, 0, 0],
rotation=[-1, 0, 0, 0],
category="cat",
attributes={"gender": "male"},
instance="12345",
)
assert labeledbox3d.__rmul__(quaternion_1) == LabeledBox3D(
size=size,
translation=[1.7999999999999996, 2, 2.6],
rotation=[-2, 1, 4, -3],
category="cat",
attributes={"gender": "male"},
instance="12345",
)
assert labeledbox3d.__rmul__(1) == NotImplemented
def rate_to_quat(self, omega, dt):
"""Rotation quaternion from gyroscope sample
Args:
omega (numpy.ndarray): angular velocity vector [x,y,z]. Same as scaled gyro sample in rad/s.
dt (float): Time delta between gyro samples for angle integration.
Returns:
numpy.ndarray: Rotation quaternion corresponding to orientation change
"""
# https://stackoverflow.com/questions/24197182/efficient-quaternion-angular-velocity/24201879#24201879
# no idea how it fully works, but it does
ha = omega * dt * 0.5
l = np.sqrt(ha.dot(ha))
if l > 1.0e-12:
ha *= np.sin(l) / l
q0 = np.cos(l)
q1 = ha[0]
q2 = ha[1]
q3 = ha[2]
return quat.normalize(quat.quaternion(q0, q1, q2, q3))
else:
return quat.quaternion(1, 0, 0, 0)
def quat_from_two_vectors(v0: np.ndarray, v1: np.ndarray) -> qt.quaternion:
r"""Creates a quaternion that rotates the first vector onto the second vector
:param v0: The starting vector, does not need to be a unit vector
:param v1: The end vector, does not need to be a unit vector
:return: The quaternion
Calculates the quaternion q such that
.. code:: py
v1 = quat_rotate_vector(q, v0)
"""
v0 = v0 / np.linalg.norm(v0)
v1 = v1 / np.linalg.norm(v1)
c = v0.dot(v1)
if c < (-1 + 1e-8):
c = max(c, -1)
m = np.stack([v0, v1], 0)
_, _, vh = np.linalg.svd(m, full_matrices=True)
axis = vh[2]
w2 = (1 + c) * 0.5
w = np.sqrt(w2)
axis = axis * np.sqrt(1 - w2)
return qt.quaternion(w, *axis)
axis = np.cross(v0, v1)
s = np.sqrt((1 + c) * 2)
return qt.quaternion(s * 0.5, *(axis / s))
def compute_delta_angle(time_stamp, position, orientation, sample_points=None,
local_axis=quaternion.quaternion(1.0, 0., 0., -1.)):
"""
Compute the cosine between the moving direction and viewing direction
:param time_stamp: Time stamp
:param position: Position. When passing Nx2 array, compute ignore z direction
:param orientation: Orientation as quaternion
:param local_axis: the viewing direction in the device frame. Default is set w.r.t. to android coord frame
:return:
"""
if sample_points is None:
sample_points = np.arange(0, time_stamp.shape[0], dtype=int)
epsilon = 1e-10
speed_dir = compute_speed(time_stamp, position)
speed_dir = np.concatenate([np.zeros([1, position.shape[1]]), speed_dir], axis=0)
speed_mag = np.linalg.norm(speed_dir, axis=1)
cos_array = np.zeros(sample_points.shape[0], dtype=float)
valid_array = np.empty(sample_points.shape[0], dtype=bool)
for i in range(sample_points.shape[0]):
if speed_mag[sample_points[i]] <= epsilon:
valid_array[i] = False
else:
q = quaternion.quaternion(*orientation[sample_points[i]])
camera_axis = (q * local_axis * q.conj()).vec[:position.shape[1]]
cos_array[i] = min(np.dot(speed_dir[sample_points[i]], camera_axis) / speed_mag[sample_points[i]], 1.0)
valid_array[i] = True
return cos_array, valid_array
def test_quaternion_subtract(Qs):
for q in Qs[Qs_finite]:
for p in Qs[Qs_finite]:
assert q - p == quaternion.quaternion(q.w - p.w, q.x - p.x, q.y - p.y, q.z - p.z)
for q in Qs[Qs_nonnan]:
for s in [-3, -2.3, -1.2, -1.0, 0.0, 0, 1.0, 1, 1.2, 2.3, 3]:
assert (q - s == quaternion.quaternion(q.w - s, q.x, q.y, q.z))
assert (s - q == quaternion.quaternion(s - q.w, -q.x, -q.y, -q.z))
def get_relative_rotation(src_orientation, dest_orientation):
a = quaternion.quaternion(src_orientation[0], src_orientation[1],
src_orientation[2], src_orientation[3])
b = quaternion.quaternion(dest_orientation[0], dest_orientation[1],
dest_orientation[2], dest_orientation[3])
relative_rotation = quaternion.as_float_array(b * a.inverse())
return relative_rotation
def test_set_rotation(self):
transform = Transform3D()
transform.set_rotation([0, 1, 0, 0])
assert transform.rotation == quaternion(0, 1, 0, 0)
quaternion_1 = quaternion(0, 1, 0, 0)
transform.set_rotation(quaternion_1)
assert transform.rotation == quaternion_1
def corotating_frame(W, R0=quaternion.one, tolerance=1e-12, z_alignment_region=None, return_omega=False):
"""Return rotor taking current mode frame into corotating frame
This function simply evaluates the angular velocity of the waveform, and
then integrates it to find the corotating frame itself. This frame is
defined to be the frame in which the time-dependence of the waveform is
minimized --- at least, to the extent possible with a time-dependent
rotation. This frame is only unique up to a single overall rotation, which
is passed in as an optional argument to this function.
Parameters
----------
W: Waveform
R0: quaternion [defaults to 1]
Value of the output rotation at the first output instant
tolerance: float [defaults to 1e-12]
Absolute tolerance used in integration
z_alignment_region: None or 2-tuple of floats [defaults to None]
If not None, the dominant eigenvector of the <LL> matrix is aligned with the z axis,
averaging over this portion of the data. The first and second elements of the input are
considered fractions of the inspiral at which to begin and end the average. For example,
(0.1, 0.9) would lead to starting 10% of the time from the first time step to the max norm
time, and ending at 90% of that time.
return_omega: bool [defaults to False]
If True, return a 2-tuple consisting of the frame (the usual returned object) and the
angular-velocity data. That is frequently also needed, so this is just a more efficient
way of getting the data.
"""
from quaternion.quaternion_time_series import integrate_angular_velocity, squad
omega = angular_velocity(W)
t, frame = integrate_angular_velocity((W.t, omega), t0=W.t[0], t1=W.t[-1], R0=R0, tolerance=tolerance)
if z_alignment_region is None:
correction_rotor = quaternion.one
else:
initial_time = W.t[0]
inspiral_time = W.max_norm_time() - initial_time
t1 = initial_time + z_alignment_region[0] * inspiral_time
t2 = initial_time + z_alignment_region[1] * inspiral_time
i1 = np.argmin(np.abs(W.t - t1))
i2 = np.argmin(np.abs(W.t - t2))
R = frame[i1:i2]
i1m = max(0, i1 - 10)
i1p = i1m + 21
RoughDirection = omega[i1m + 10]
Vhat = W[i1:i2].LLDominantEigenvector(RoughDirection=RoughDirection, RoughDirectionIndex=0)
Vhat_corot = np.array([(Ri.conjugate() * quaternion.quaternion(*Vhati) * Ri).vec for Ri, Vhati in zip(R, Vhat)])
Vhat_corot_mean = quaternion.quaternion(*np.mean(Vhat_corot, axis=0)).normalized()
correction_rotor = np.sqrt(-quaternion.z * Vhat_corot_mean).inverse()
frame = frame * correction_rotor
frame = frame / np.abs(frame)
if return_omega:
return (frame, omega)
else:
return frame
def get_grasp_pose(self, theta):
q = quaternion(0, 1, 0, 0)
cos = np.cos(theta / 2)
sin = np.sin(theta / 2)
p = quaternion(cos, 0, 0, sin)
rot_qt = p * q
print("Theta", rot_qt, cos, sin, theta / 2)
rot_arr = qn.as_float_array(rot_qt)
rot_qt = [rot_arr[1], rot_arr[2], rot_arr[3], rot_arr[0]]
return rot_qt
def set_rotation(self, rotation: RotationType) -> None:
"""Set the rotation of the transform.
Arguments:
rotation: Rotation in a sequence of [w, x, y, z] or numpy quaternion.
"""
if isinstance(rotation, quaternion):
self._rotation = quaternion(rotation)
else:
self._rotation = quaternion(*rotation)
def correct_gyro_drifting(t, rv, magnet):
assert t.shape[0] == rv.shape[0]
assert t.shape[0] == magnet.shape[0]
d_rot = np.empty([rv.shape[0], 4], dtype=float)
rv_quats = []
for r in rv:
rv_quats.append(quaternion.quaternion(*r))
rv_dev = [quaternion.quaternion(1.0, 0.0, 0.0, 0.0)]
for i in range(1, rv.shape[0]):
d_rot[i] = quaternion.as_float_array()
def set_traj(self, traj, quat):
q_init = quaternion.quaternion(quat.w_val, quat.x_val, quat.y_val,
quat.z_val).normalized()
assert (abs(q_init.norm() - 1) < 0.0001)
for quat in traj:
converted = q_init * quaternion.quaternion(quat[3], quat[4],
quat[5], quat[6])
quat[3:7] = [converted.w, converted.x, converted.y, converted.z]
# quat[11:] = quaternion.rotate_vectors(converted, quat[11:])
self.reference = traj
self.reset_errors()
def test_rmul(self):
transform = Transform3D(translation=[1, 2, 3],
rotation=quaternion(0, 1, 0, 0))
quaternion_1 = quaternion(1, 2, 3, 4)
box3d = Box3D(transform)
assert box3d.__rmul__(transform) == Box3D(translation=[2, 0, 0],
rotation=quaternion(
-1, 0, 0, 0))
assert box3d.__rmul__(quaternion_1) == Box3D(
translation=[1.7999999999999996, 2, 2.6],
rotation=quaternion(-2, 1, 4, -3))
assert box3d.__rmul__(1) == NotImplemented
def __init__(self, time_stamp, orientation, linacce, cos_array, speed_ind,
variable_ind):
super().__init__(time_stamp, orientation, linacce, variable_ind)
self.speed_ind_ = speed_ind
self.cos_array_ = cos_array
self.camera_axis_ = np.empty(linacce.shape, dtype=float)
camera_axis_local = quaternion.quaternion(1., 0., 0., -1.)
for i in range(linacce.shape[0]):
q = quaternion.quaternion(*orientation[i])
self.camera_axis_[i] = (q * camera_axis_local *
q.conj()).vec[:linacce.shape[1]]
self.camera_axis_ /= np.linalg.norm(self.camera_axis_, axis=1)[:, None]
def test_set_rotation(self):
transform = Transform3D()
transform.set_rotation(0, 1, 0, 0)
assert transform.rotation == quaternion(0, 1, 0, 0)
quaternion_1 = quaternion(0, 1, 0, 0)
transform.set_rotation(quaternion=quaternion_1)
assert transform.rotation == quaternion_1
with pytest.raises(TypeError):
transform.set_rotation([0, 1, 0, 0])
def rotate(px, py, pz, ccAngleinDegree, axis_i, axis_j, axis_k):
q = quaternion(ccAngleinDegree, axis_i, axis_j, axis_k)
q.lsq()
q_revert = quaternion(-ccAngleinDegree, axis_i, axis_j, axis_k)
p = array.array('d', [px, py, pz])
print("p=", p)
pq = q * p
print("pxq= ", pq)
q_revert.lsq()
p_new = pq * q_revert
print("p_new=: ", p_new)
return p_new
def rotate_quaternion(self, axis, v, theta):
vector = np.array([0.] + v)
rot_axis = np.array([0.] + axis)
axis_angle = (theta * 0.5) * rot_axis / np.linalg.norm(rot_axis)
vec = quat.quaternion(*v)
qlog = quat.quaternion(*axis_angle)
q = np.exp(qlog)
for i in range(self.compute_point):
result = q * vec * np.conjugate(q)
return result
def test_quaternion_add(Qs):
for j in Qs_nonnan:
for k in Qs_nonnan:
q = Qs[j]
p = Qs[k]
assert (q + p == quaternion.quaternion(q.w + p.w, q.x + p.x, q.y + p.y, q.z + p.z)
or (j == q_inf1 and k == q_minf1)
or (k == q_inf1 and j == q_minf1))
for q in Qs[Qs_nonnan]:
for s in [-3, -2.3, -1.2, -1.0, 0.0, 0, 1.0, 1, 1.2, 2.3, 3]:
assert (q + s == quaternion.quaternion(q.w + s, q.x, q.y, q.z))
assert (s + q == quaternion.quaternion(q.w + s, q.x, q.y, q.z))
def drehen3D(objekt, winkelwerte):
for j, theta in enumerate(winkelwerte):
#j = 0-2 und damit auch gleich aus objekt[0-2] die Objektachsen
theta = np.radians(theta)
axis_angle = (theta * 0.5) * objekt[j] / np.linalg.norm(objekt[j])
for i, punkt in enumerate(objekt):
vec = quat.quaternion(*punkt)
qlog = quat.quaternion(*axis_angle)
q = np.exp(qlog)
vec = q * vec * np.conjugate(q)
objekt[i] = np.array([0, *vec.imag])
return objekt[3:]
def load(self, data_path):
if data_path[-1] == '/':
data_path = data_path[:-1]
with open(osp.join(data_path, 'info.json')) as f:
self.info = json.load(f)
# {'tango_reference_time': 3697949488366.0, 'date': '01/21/19', 'imu_init_gyro_bias': [0.005569000000000001, 0.009201, -0.00018299999999999914], 'imu_acce_scale': [0.9987918889533104, 0.997062129866083, 0.9932574091553678], 'grv_ori_error': 8.353779492444412, 'align_tango_to_body': [-0.4841758535176575, -0.4938374978693588, -0.5424326634072199, 0.47693298715598254], 'start_frame': 5896, 'imu_end_gyro_bias': [0.005432, 0.008774, -4.6e-05], 'type': 'annotated', 'imu_reference_time': 3610942440968.0, 'start_calibration': [0.0, 0.99995545, 0.009439, 0.0], 'ekf_ori_error': 3.6239102945197676, 'imu_acce_bias': [-0.15661902624553775, -0.026333329541761968, 0.05681364453654479], 'gyro_integration_error': 8.481689436777705, 'device': 'asus4', 'length': 323.7550000070669, 'imu_time_offset': -0.07619643889847794, 'end_calibration': [0.0, 0.99998599, 0.0052938, 0.0]}
self.info['path'] = osp.split(data_path)[-1]
self.info['ori_source'], ori, self.info['source_ori_error'] = select_orientation_source( # ori is hdf5.synced.game_rv i. e. Android Sensor.TYPE_GAME_ROTATION_VECTOR
data_path, self.max_ori_error, self.grv_only)
#('game_rv', array([[ 0.02301384, -0.734161 , 0.00956859, 0.67851714], [ 0.02296023, -0.73417201, 0.00956628, 0.67850771], ..., [ 0.05427992, 0.70881762, 0.07637797, -0.6991363 ]]]), 8.353779492444412)
with h5py.File(osp.join(data_path, 'data.hdf5')) as f:
gyro_uncalib = f['synced/gyro_uncalib'] # <HDF5 dataset "gyro_uncalib": shape (64752, 3), type "<f8">
acce_uncalib = f['synced/acce'] # <HDF5 dataset "acce": shape (64752, 3), type "<f8">
gyro = gyro_uncalib - np.array(self.info['imu_init_gyro_bias']) # array([[-0.02230625, 0.01376489, 0.01876768], [-0.01673078, 0.00785385, 0.01808999], ..., [ 0.00657855, -0.02807542, 0.00804713]])
acce = np.array(self.info['imu_acce_scale']) * (acce_uncalib - np.array(self.info['imu_acce_bias'])) # array([[-9.76895341, -0.19332236, -0.85234999], [-9.76140265, -0.2099069 , -0.81018915], ..., [-9.82066284, -0.32593967, -0.28265888]])
ts = np.copy(f['synced/time']) # array([3641.39639807, 3641.40139807, 3641.40639807, ..., 3965.14139807, 3965.14639807, 3965.15139807])
tango_pos = np.copy(f['pose/tango_pos']) # array([[ 0.00747055, 0.0794231 , 0.04721916], [ 0.00743938, 0.07954534, 0.04721213], ..., [ 0.04869788, -0.01891041, -0.03532039]])
init_tango_ori = quaternion.quaternion(*f['pose/tango_ori'][0]) # quaternion(0.500218919180744, 0.498520940104168, 0.458115146317795, -0.539803994906776)
# Compute the IMU orientation in the Tango coordinate frame.
ori_q = quaternion.from_float_array(ori) # array([quaternion(0.0230138445473811, -0.734161004581412, 0.00956858773770847, 0.678517142961637), quaternion(0.0229602307881793, -0.734172007659053, 0.00956628356173319, 0.678507709011024), ..., quaternion(0.0542799190079345, 0.708817623072373, 0.0763779734707989, -0.6991362963311)], dtype=quaternion)
# hdf5.synced.game_rv i. e. Android Sensor.TYPE_GAME_ROTATION_VECTOR
rot_imu_to_tango = quaternion.quaternion(*self.info['start_calibration']) # quaternion(0, 0.99995545, 0.009439, 0)
init_rotor = init_tango_ori * rot_imu_to_tango * ori_q[0].conj() # quaternion(-0.695289552060529, 0.00118374652078425, 0.00248606386725569, 0.718723783950829)
ori_q = init_rotor * ori_q # array([quaternion(-0.5028224217168, 0.50529138394065, -0.535057892743795, -0.453388785030156), quaternion(-0.502778345472314, 0.505300603413145, -0.535064320967734, -0.453420734559951), ..., quaternion(0.463716682908265, -0.54940199756135, 0.457301820727505, 0.523442677366401)], dtype=quaternion)
# ori_q = f['pose/tango_ori'][0] * self.info['start_calibration'] * conj(f['synced/game_rv'][0]) * f['synced/game_rv']
dt = (ts[self.w:] - ts[:-self.w])[:, None] # array([[1.], [1.], [1.], ..., [1.],[1.], [1.]])
glob_v = (tango_pos[self.w:] - tango_pos[:-self.w]) / dt # array([[-0.00533056, 0.01667982, -0.00509732], [-0.00531125, 0.016594 , -0.00511179], ..., [-0.0023489 , 0.00633583, -0.00057166]])
# these two below are position vector arrays
gyro_q = quaternion.from_float_array(np.concatenate([np.zeros([gyro.shape[0], 1]), gyro], axis=1)) # array([quaternion(0, -0.0223062507369463, 0.0137648946088728, 0.0187676819234896), quaternion(0, -0.0167307794589504, 0.00785385399152264, 0.018089989505323), ..., quaternion(0, 0.00657855020053501, -0.0280754170707831, 0.0080471337151681)], dtype=quaternion)
acce_q = quaternion.from_float_array(np.concatenate([np.zeros([acce.shape[0], 1]), acce], axis=1)) # array([quaternion(0, -9.76895340810378, -0.193322357928738, -0.852349993053583), quaternion(0, -9.76140265037272, -0.209906902110648, -0.810189151712629), ..., quaternion(0, -9.8206628384554, -0.325939671417927, -0.282658875290474)], dtype=quaternion)
# each element vector rotated by the corresponding ori_q rotation quaternion
# At test time, we use the coordinate frame defined by system device orientations from Android or iOS, whose Z axis is aligned with gravity.
# The whole is transformed into the global (Tango) coordinate frame.
glob_gyro = quaternion.as_float_array(ori_q * gyro_q * ori_q.conj())[:, 1:] # array([[-0.01258328, 0.02161022, 0.02034508], [-0.00665554, 0.02000185, 0.0149826 ], ..., [ 0.02674353, 0.01209917, -0.0058859 ]])
glob_acce = quaternion.as_float_array(ori_q * acce_q * ori_q.conj())[:, 1:] # array([[-3.46650073e-02, -3.36474233e-02, 9.80783499e+00], [-1.38849800e-02, 6.54256497e-03, 9.79719413e+00], ..., [ 3.31727211e-02, -6.26925428e-02, 9.82980926e+00]])
start_frame = self.info.get('start_frame', 0) # 5896
self.ts = ts[start_frame:] # array([3670.87639807, 3670.88139807, 3670.88639807, ..., 3965.14139807, 3965.14639807, 3965.15139807])
self.features = np.concatenate([glob_gyro, glob_acce], axis=1)[start_frame:] # array([[-5.94662071e-03, -9.38751552e-03, -5.15188486e-03, -3.08588928e-02, 6.39869105e-02, 9.86019268e+00], [-5.27530580e-03, -7.75847573e-03, -1.59778536e-02, -3.54599110e-02, 5.16253587e-02, 9.82394159e+00], ..., [ 2.67435308e-02, 1.20991655e-02, -5.88589595e-03, 3.31727211e-02, -6.26925428e-02, 9.82980926e+00]])
self.targets = glob_v[start_frame:, :2] # array([[-0.02427537, 0.02117807], [-0.02406481, 0.02145767], ..., [-0.0023489 , 0.00633583]]) targets are averaged for the window w
self.orientations = quaternion.as_float_array(ori_q)[start_frame:] # array([[-0.51946022, 0.24279321, -0.60182678, 0.55589147], [-0.51947897, 0.24272502, -0.6018242 , 0.55590699], ..., [ 0.46371668, -0.549402 , 0.45730182, 0.52344268]])
self.gt_pos = tango_pos[start_frame:] # array([[ 0.17387274, -0.14344794, -0.0743621 ], [ 0.17362087, -0.14350179, -0.07425673], ..., [ 0.04869788, -0.01891041, -0.03532039]])
def rotate_vector(input, orientation):
"""
Rotate 3D vectors with quaternions.
:param input: Nx3 array containing N 3D vectors.
:param orientation: Nx4 array containing N quaternions.
:return: Nx3 array containing rotated vectors.
"""
output = np.empty(input.shape, dtype=float)
for i in range(input.shape[0]):
q = quaternion.quaternion(*orientation[i])
output[i] = (q * quaternion.quaternion(1.0, *input[i]) * q.conj()).vec
return output
def bislerp(
Qa: np.ndarray,
Qb: np.ndarray,
t: float,
) -> np.ndarray:
r"""
Linear interpolation of two orthogonal matrices.
Assiume that :math:`Q_a` and :math:`Q_b` refers to
timepoint :math:`0` and :math:`1` respectively.
Using spherical linear interpolation (slerp) find the
orthogonal matrix at timepoint :math:`t`.
"""
if ~Qa.any() and ~Qb.any():
return np.zeros((3, 3))
if ~Qa.any():
return Qb
if ~Qb.any():
return Qa
tol = 1e-12
qa = quaternion.from_rotation_matrix(Qa)
qb = quaternion.from_rotation_matrix(Qb)
quat_i = quaternion.quaternion(0, 1, 0, 0)
quat_j = quaternion.quaternion(0, 0, 1, 0)
quat_k = quaternion.quaternion(0, 0, 0, 1)
quat_array = [
qa,
-qa,
qa * quat_i,
-qa * quat_i,
qa * quat_j,
-qa * quat_j,
qa * quat_k,
-qa * quat_k,
]
dot_arr = [abs((qi.components * qb.components).sum()) for qi in quat_array]
max_idx = int(np.argmax(dot_arr))
max_dot = dot_arr[max_idx]
qm = quat_array[max_idx]
if max_dot > 1 - tol:
return Qb
qm_slerp = quaternion.slerp(qm, qb, 0, 1, t)
qm_norm = qm_slerp.normalized()
Qab = quaternion.as_rotation_matrix(qm_norm)
return Qab
def bislerp(Qa, Qb, t):
r"""
Linear interpolation of two orthogonal matrices.
Assiume that :math:`Q_a` and :math:`Q_b` refers to
timepoint :math:`0` and :math:`1` respectively.
Using spherical linear interpolation (slerp) find the
orthogonal matrix at timepoint :math:`t`.
"""
if Qa is None and Qb is None:
return None
if Qa is None:
return Qb
if Qb is None:
return Qa
tol = 1e-12
qa = quaternion.from_rotation_matrix(Qa)
qb = quaternion.from_rotation_matrix(Qb)
quat_i = quaternion.quaternion(0, 1, 0, 0)
quat_j = quaternion.quaternion(0, 0, 1, 0)
quat_k = quaternion.quaternion(0, 0, 0, 1)
quat_array = [
qa,
-qa,
qa * quat_i,
-qa * quat_i,
qa * quat_j,
-qa * quat_j,
qa * quat_k,
-qa * quat_k,
]
def dot(qi, qj):
return np.sum(
[getattr(qi, s) * getattr(qj, s) for s in ["x", "y", "z", "w"]])
dot_arr = [abs(dot(qi, qb)) for qi in quat_array]
max_idx = np.argmax(dot_arr)
max_dot = dot_arr[max_idx]
qm = quat_array[max_idx]
if max_dot > 1 - tol:
return Qb
else:
qm_slerp = quaternion.slerp(qm, qb, 0, 1, t)
qm_norm = qm_slerp.normalized()
Qab = quaternion.as_rotation_matrix(qm_norm)
return Qab
def test_quaternion_members():
Q = quaternion.quaternion(1.1, 2.2, 3.3, 4.4)
assert Q.real == 1.1
assert Q.w == 1.1
assert Q.x == 2.2
assert Q.y == 3.3
assert Q.z == 4.4
def conjugation(q,p):
qc = q.copy().conjugate()
print p[0], p[1], p[2]
pq = quaternion(r=0,i=p[0],j=p[1],k=p[2])
r = q*pq*qc
print r
return r.get()[1:]
def maritime_hall_figure(fig_num, dem_file, reference_trajectory,
slam_trajectory,
mis_orient = quat.quaternion(np.array([1.0,0.0,0.0,0.0])),
mis_position = [0.00, 0.00, 0.00]):
import os
from matplotlib.cbook import get_sample_data
from matplotlib._png import read_png
import matplotlib.image as image
matplotlib.rcParams.update({'font.size': 15, 'font.weight': 'bold'})
#fig = plt.figure(fig_num, figsize=(28, 16), dpi=120, facecolor='w', edgecolor='k')
#ax = fig.add_subplot(111)
fn = get_sample_data(os.getcwd()+"/data/img/maritime_hall.png", asfileobj=False)
maritime_hall = image.imread(fn)
fig, ax = plt.subplots()
ax.imshow(maritime_hall, extent=[-1.2, 25, -2, 20])
#ax.imshow(maritime_hall, extent=[-1.2, 25, -2, 19])
# Display Ground Truth trajectory
from numpy import linalg as la
ref = np.column_stack((reference_trajectory[:,0][0::1], reference_trajectory[:,1][0::1], reference_trajectory[:,2][0::1]))
ref[:] = [(mis_orient * i * mis_orient.conj())[1:4] for i in ref ]
ref[:] = [ i + mis_position for i in ref ]
x = ref[:,0]
y = ref[:,1]
ax.plot(x, y, marker='D', linestyle='-', lw=2, alpha=0.3, color=[1.0, 1.0, 0.0],
label='ground truth', zorder=80)
# Annotations
from matplotlib.offsetbox import OffsetImage, AnnotationBbox
ax.annotate(r'Start', xy=(x[0], y[0]), xycoords='data',
xytext=(-5, 5), textcoords='offset points', fontsize=16,
horizontalalignment='left',
verticalalignment='bottom',
zorder=101
)
ax.scatter(x[0], y[0], marker='o', facecolor='k', s=40, alpha=1.0, zorder=103)
ax.annotate(r'End', xy=(x[x.shape[0]-1], y[y.shape[0]-1]), xycoords='data',
xytext=(-5, 5), textcoords='offset points',
fontsize=16,
horizontalalignment='left',
verticalalignment='bottom',
zorder=101
)
ax.scatter(x[x.shape[0]-1], y[y.shape[0]-1], marker='o', facecolor='k', s=40, alpha=1.0, zorder=103)
plt.xlabel(r'X [$m$]', fontsize=15, fontweight='bold')
plt.ylabel(r'Y [$m$]', fontsize=15, fontweight='bold')
ax.legend(loc=1, prop={'size':15})
plt.grid(True)
fig.savefig("gt_maritime_hall_20180720-1644.png", dpi=fig.dpi)
plt.show(block=True)
def __init__(self,filename,istrj = True,coarse_grain=False):
self.r = 0 # default scaling factor for system
self.pos = numpy.zeros(3) # center of bounding box
self.orien = quaternion([0,0,-1,0]) # orientation in space
self.scaleFactor = 1
self.idx = None
self.DirV = []
self.istrj = istrj
self.coarse_grain = coarse_grain
self.clipplane = numpy.array([0.,0.,0.,0,], numpy.float32)
self.excl = numpy.array([], numpy.int32)
if not istrj: self.load_pdb(filename)
else: self.load_trj(filename)
def readData(self, filename, angle_axis=False, cov=False):
for row in csv.reader(open(os.path.expanduser(filename), "r"), delimiter=" ", quotechar="|"):
# print row
self.atime.append(float(row[0]) / 1000000.00) # absolute time
if False != angle_axis:
self.data.append(
quat.quaternion.fromAngleAxis(float(row[4]), [float(row[1]), float(row[2]), float(row[3])])
)
else:
self.data.append(quat.quaternion([float(row[4]), float(row[1]), float(row[2]), float(row[3])]))
if False != cov:
matrix = np.array(
[
[float(row[5]), float(row[6]), float(row[7])],
[float(row[8]), float(row[9]), float(row[10])],
[float(row[11]), float(row[12]), float(row[13])],
]
)
self.cov.append(matrix)
atime = self.atime
self.time.append(0.00)
for i in range(0, len(atime) - 1):
tbody = float(atime[i + 1]) - float(atime[i])
self.delta.append(tbody)
tbody = float(atime[i + 1]) - float(atime[0])
self.time.append(tbody)
if len(self.delta) > 1:
self.delta.append(self.delta[len(self.delta) - 1])
self.t = mean(self.delta) * r_[0 : len(self.atime)]
# Convert to np array
self.atime = np.asarray(self.atime)
self.time = np.asarray(self.time)
self.delta = np.asarray(self.delta)
self.t = np.asarray(self.t)
self.cov = np.asarray(self.cov)
def delete(self, index_to_remove):
"""Delete internal data from the index specified in temindex """
indexes = np.setdiff1d(xrange(len(self.data)), index_to_remove)
self.atime = self.atime[indexes]
self.time = self.time[indexes]
self.delta = self.delta[indexes]
self.t = self.t[indexes]
# convert quaternion to array and remove outliers
dataq = np.asarray(self.data)
dataq = dataq[indexes]
# Get back to list of quaternions
self.data = []
for q in dataq:
self.data.append(quat.quaternion([float(q[0]), float(q[1]), float(q[2]), float(q[3])]))
if len(self.cov) > 0:
self.cov = self.cov[indexes]
if len(self.var) > 0:
self.var = self.var[indexes]
def test_quaternion_multiply(Qs):
# Check scalar multiplication
for q in Qs[Qs_finite]:
assert q * Qs[q_1] == q
for q in Qs[Qs_finite]:
assert q * 1.0 == q
assert q * 1 == q
assert 1.0 * q == q
assert 1 * q == q
for s in [-3, -2.3, -1.2, -1.0, 0.0, 0, 1.0, 1, 1.2, 2.3, 3]:
for q in Qs[Qs_finite]:
assert q * s == quaternion.quaternion(s * q.w, s * q.x, s * q.y, s * q.z)
assert s * q == q * s
for q in Qs[Qs_finite]:
assert 0.0 * q == Qs[q_0]
assert 0.0 * q == q * 0.0
# Check linearity
for q1 in Qs[Qs_finite]:
for q2 in Qs[Qs_finite]:
for q3 in Qs[Qs_finite]:
assert allclose(q1*(q2+q3), (q1*q2)+(q1*q3))
assert allclose((q1+q2)*q3, (q1*q3)+(q2*q3))
# Check the multiplication table
for q in [Qs[q_1], Qs[x], Qs[y], Qs[z]]:
assert Qs[q_1] * q == q
assert q * Qs[q_1] == q
assert Qs[x] * Qs[x] == -Qs[q_1]
assert Qs[x] * Qs[y] == Qs[z]
assert Qs[x] * Qs[z] == -Qs[y]
assert Qs[y] * Qs[x] == -Qs[z]
assert Qs[y] * Qs[y] == -Qs[q_1]
assert Qs[y] * Qs[z] == Qs[x]
assert Qs[z] * Qs[x] == Qs[y]
assert Qs[z] * Qs[y] == -Qs[x]
assert Qs[z] * Qs[z] == -Qs[q_1]
def decos_trajectories_figure(fig_num, dem_file, reference_trajectory,
kf_trajectory, frames_trajectory, odo_trajectory,
mis_orient = quat.quaternion(np.array([1.0,0.0,0.0,0.0])),
mis_position = [0.00, 0.00, 0.00]):
########################
# Load Terrain DEM
########################
import os
plydata = PlyData.read(open(os.path.expanduser(dem_file)))
vertex = plydata['vertex'].data
[dem_px, dem_py, dem_pz] = (vertex[t] for t in ('x', 'y', 'z'))
# define grid.
npts=100
dem_xi = np.linspace(min(dem_px), max(dem_px), npts)
dem_yi = np.linspace(min(dem_py), max(dem_py), npts)
# grid the data.
dem_zi = griddata(dem_px, dem_py, dem_pz, dem_xi, dem_yi, interp='linear')
#################################################
# Misalignment all trajectories wrt to the map
#################################################
map_posi_align = [1.00, 5.00, 0.00]
map_orient_align = quat.quaternion.fromAngleAxis(-25.0 * np.pi/180.0, [0.0, 0.0,1.0])
matplotlib.rcParams.update({'font.size': 15, 'font.weight': 'bold'})
fig = plt.figure(fig_num, figsize=(28, 16), dpi=120, facecolor='w', edgecolor='k')
ax = fig.add_subplot(111)
#fig, ax = plt.subplots()
# Display the DEM
plt.rc('text', usetex=False)# activate latex text rendering
CS = plt.contour(dem_xi, dem_yi, dem_zi, 20, linewidths=0.5, colors='k')
CS = plt.contourf(dem_xi, dem_yi, dem_zi, 20, cmap=plt.cm.gray, vmax=abs(dem_zi).max(), vmin=-abs(dem_zi).max())
#plt.clabel(CS, inline=1, fontsize=10) # number in the contour line
# plot data points.
plt.xlim(min(dem_px), max(dem_xi))
plt.ylim(min(dem_py), max(dem_yi))
# Color bar of the dem
cbar = plt.colorbar() # draw colorbar
cbar.ax.set_ylabel(r' terrain elevation[$m$] ', fontsize=25, fontweight='bold')
# Display Ground Truth trajectory
from numpy import linalg as la
ref = np.column_stack((reference_trajectory[:,0][0::10], reference_trajectory[:,1][0::10], reference_trajectory[:,2][0::10]))
ref[:] = [(map_orient_align * i * map_orient_align.conj())[1:4] for i in ref ]
ref[:] = [ i + map_posi_align for i in ref ]
x = ref[:,0]
y = ref[:,1]
ax.plot(x, y, marker='D', linestyle='-', lw=2, alpha=0.3, color=[1.0, 1.0, 0.0],
label='ground truth', zorder=80)
# Plot all the image frames
fr = np.column_stack((frames_trajectory[:,0][0::10], frames_trajectory[:,1][0::10], frames_trajectory[:,2][0::10]))
fr[:] = [(mis_orient * i * mis_orient.conj())[1:4] for i in fr]
fr[:] = [ i + mis_position for i in fr ]
fr[:] = [(map_orient_align * i * map_orient_align.conj())[1:4] for i in fr ]
fr[:] = [ i + map_posi_align for i in fr ]
fr_x = fr[:,0]
fr_y = fr[:,1]
ax.plot(fr_x, fr_y, marker='s', linestyle='-', lw=2, alpha=0.3, color=[0.0, 0.3, 1.0],
label='slam', zorder=99)
# Plot the key frames
kf = np.column_stack((kf_trajectory[:,0], kf_trajectory[:,1], kf_trajectory[:,2]))
kf[:] = [(mis_orient * i * mis_orient.conj())[1:4] for i in kf]
kf[:] = [ i + mis_position for i in kf ]
kf[:] = [(map_orient_align * i * map_orient_align.conj())[1:4] for i in kf ]
kf[:] = [ i + map_posi_align for i in kf ]
kf_x = kf[:,0][0::10]
kf_y = kf[:,1][0::10]
ax.scatter(kf_x, kf_y, marker='s', facecolor=[0.2,1.0,0.0], edgecolor='b',
label='keyframes', s=20, alpha=1.0, zorder=100)
# Pure odometry trajectory
odo = np.column_stack((odo_trajectory[:,0], odo_trajectory[:,1], odo_trajectory[:,2]))
odo[:] = [(mis_orient * i * mis_orient.conj())[1:4] for i in odo ]
odo[:] = [ i + mis_position for i in odo ]
odo[:] = [(map_orient_align * i * map_orient_align.conj())[1:4] for i in odo ]
odo[:] = [ i + map_posi_align for i in odo ]
odo_x = odo[:,0][0::10]
odo_y = odo[:,1][0::10]
ax.plot(odo_x, odo_y, marker='h', linestyle='-', lw=2, alpha=0.3, color=[1.0, 0, 0],
label='odometry', zorder=70)
import os
from matplotlib.cbook import get_sample_data
from matplotlib._png import read_png
import matplotlib.image as image
from scipy import ndimage
from matplotlib.offsetbox import OffsetImage, AnnotationBbox
fn = get_sample_data(os.getcwd()+"/data/img/exoter.png", asfileobj=False)
exoter = image.imread(fn)
exoter = ndimage.rotate(exoter, -120)
imexoter = OffsetImage(exoter, zoom=0.3)
ab = AnnotationBbox(imexoter, xy=(x[0], y[0]),
xybox=None,
xycoords='data',
boxcoords="offset points",
frameon=False)
ax.annotate(r'ExoTeR', xy=(x[0], y[0]), xycoords='data',
xytext=(-20, -40), textcoords='offset points',
fontsize=16)
ax.annotate(r'Start', xy=(x[0], y[0]), xycoords='data',
xytext=(-5, 5), textcoords='offset points', fontsize=16,
horizontalalignment='left',
verticalalignment='bottom',
zorder=101
)
ax.scatter(x[0], y[0], marker='o', facecolor='k', s=40, alpha=1.0, zorder=103)
#ax.arrow(x[0], y[0], x[13]-x[0], y[13]-y[0], width=0.01, head_width=0.05,
# head_length=0.2, fc='k', ec='k', zorder=104)
ax.annotate(r'End', xy=(x[x.shape[0]-1], y[y.shape[0]-1]), xycoords='data',
xytext=(-5, 5), textcoords='offset points',
fontsize=16,
horizontalalignment='left',
verticalalignment='bottom',
zorder=101
)
ax.scatter(x[x.shape[0]-1], y[y.shape[0]-1], marker='o', facecolor='k', s=40, alpha=1.0, zorder=103)
ax.add_artist(ab)
plt.xlabel(r'X [$m$]', fontsize=15, fontweight='bold')
plt.ylabel(r'Y [$m$]', fontsize=15, fontweight='bold')
ax.legend(loc=1, prop={'size':15})
plt.grid(True)
fig.savefig("decos_trajectories_figure_20140911-1805.png", dpi=fig.dpi)
plt.show(block=True)
def RHS(t, y):
R = quaternion.quaternion(*y)
return (0.5 * quaternion.quaternion(0.0, *Omega_func(t, R)) * R).components
# grid the data.
zi = griddata(px, py, pz, xi, yi, interp='linear')
#################################################
# Misalignment to the map #
#################################################
map_posi_align = [2.00, 8.00, 0.00]
map_orient_align = quat.quaternion.fromAngleAxis(-20.0 * np.pi/180.0, [0.0, 0.0,1.0])
#################################################
# Take the misalignment between both orientations
#################################################
#misalignment = ~odometry_orient.data[1000] * reference_orient.data[1000] #~ is invert operator
misalignment = quat.quaternion(np.array([1.0,0.0,0.0,0.0]))
##########
## PLOT ##
##########
#Position comparison X-Y plane
matplotlib.rcParams.update({'font.size': 30, 'font.weight': 'bold'})
fig = plt.figure(1)
ax = fig.add_subplot(111)
# Display the DEM
plt.rc('text', usetex=False)# activate latex text rendering
CS = plt.contour(xi, yi, zi, 25, linewidths=0.5, colors='k')
CS = plt.contourf(xi, yi, zi, 25, cmap=plt.cm.gray, vmax=abs(zi).max(), vmin=-abs(zi).max())
cbar = plt.colorbar() # draw colorbar
def decos_dem_figure(fig_num, dem_file, trajectory, pred_mean, kf_trajectory,
frames_trajectory, mis_orient = quat.quaternion(np.array([1.0,0.0,0.0,0.0])),
mis_position = [0.00, 0.00, 0.00],
color_bar='Reds'):
########################
# Load Terrain DEM
########################
import os
plydata = PlyData.read(open(os.path.expanduser(dem_file)))
vertex = plydata['vertex'].data
[dem_px, dem_py, dem_pz] = (vertex[t] for t in ('x', 'y', 'z'))
# define grid.
npts=100
dem_xi = np.linspace(min(dem_px), max(dem_px), npts)
dem_yi = np.linspace(min(dem_py), max(dem_py), npts)
# grid the data.
dem_zi = griddata(dem_px, dem_py, dem_pz, dem_xi, dem_yi, interp='linear')
#################################################
# Misalignment to the map #
#################################################
map_posi_align = [1.00, 5.00, 0.00]
map_orient_align = quat.quaternion.fromAngleAxis(-25.0 * np.pi/180.0, [0.0, 0.0,1.0])
matplotlib.rcParams.update({'font.size': 30, 'font.weight': 'bold'})
fig = plt.figure(fig_num, figsize=(28, 16), dpi=120, facecolor='w', edgecolor='k')
ax = fig.add_subplot(111)
#fig, ax = plt.subplots()
# Display the DEM
plt.rc('text', usetex=False)# activate latex text rendering
CS = plt.contour(dem_xi, dem_yi, dem_zi, 20, linewidths=0.5, colors='k')
CS = plt.contourf(dem_xi, dem_yi, dem_zi, 20, cmap=plt.cm.gray, vmax=abs(dem_zi).max(), vmin=-abs(dem_zi).max())
#plt.clabel(CS, inline=1, fontsize=10) # number in the contour line
# plot data points.
plt.xlim(min(dem_px), max(dem_xi))
plt.ylim(min(dem_py), max(dem_yi))
# Display Ground Truth trajectory
from numpy import linalg as la
ref = np.column_stack((trajectory[:,0], trajectory[:,1], trajectory[:,2]))
ref[:] = [(map_orient_align * i * map_orient_align.conj())[1:4] for i in ref ]
ref[:] = [ i + map_posi_align for i in ref ]
x = ref[:,0]
y = ref[:,1]
sd = la.norm(pred_mean, axis=1)
points = np.array([x, y]).T.reshape(-1, 1, 2)
segments = np.concatenate([points[:-1], points[1:]], axis=1)
from matplotlib.collections import LineCollection
from matplotlib.colors import ListedColormap, BoundaryNorm
from matplotlib.colors import LinearSegmentedColormap as lscm
cmap = plt.get_cmap(color_bar)
norm = plt.Normalize(0.00, 0.0634491701615)
lc = LineCollection(segments, cmap=cmap, norm=norm)
lc.set_array(sd)
lc.set_linewidth(25)
lc.set_alpha(0.8)
plt.gca().add_collection(lc)
#color bar of the covarianve
#h_cbar = plt.colorbar(lc)#, orientation='horizontal')
#h_cbar.ax.set_ylabel(r' odometry error[$m/s$]', fontsize=25, fontweight='bold')
# Color bar of the dem
#cbar = plt.colorbar() # draw colorbar
#cbar.ax.set_ylabel(r' terrain elevation[$m$]', fontsize=25, fontweight='bold')
# Plot all the image frames
fr = np.column_stack((frames_trajectory[:,0][0::10], frames_trajectory[:,1][0::10], frames_trajectory[:,2][0::10]))
fr[:] = [(mis_orient * i * mis_orient.conj())[1:4] for i in fr]
fr[:] = [ i + mis_position for i in fr ]
fr[:] = [(map_orient_align * i * map_orient_align.conj())[1:4] for i in fr ]
fr[:] = [ i + map_posi_align for i in fr ]
fr_x = fr[:,0]
fr_y = fr[:,1]
ax.plot(fr_x, fr_y,
#marker='s',
linestyle='-', lw=2, alpha=0.5, color=[0.0, 0.3, 1.0],
label='slam trajectory',
zorder=99)
# Plot all the image frames
ax.scatter(fr_x, fr_y, marker='s', facecolor=[0.0,0.3,1.0], edgecolor='b',
label='image frames', s=300, alpha=0.2, zorder=99)
# Plot the key frames
kf = np.column_stack((kf_trajectory[:,0], kf_trajectory[:,1], kf_trajectory[:,2]))
kf[:] = [(mis_orient * i * mis_orient.conj())[1:4] for i in kf]
kf[:] = [ i + mis_position for i in kf ]
kf[:] = [(map_orient_align * i * map_orient_align.conj())[1:4] for i in kf ]
kf[:] = [ i + map_posi_align for i in kf ]
kf_x = kf[:,0]
kf_y = kf[:,1]
ax.scatter(kf_x, kf_y, marker='D', facecolor=[0.2,1.0,0.0], edgecolor='b',
label='keyframes', s=250, alpha=0.8, zorder=100)
import os
from matplotlib.cbook import get_sample_data
from matplotlib._png import read_png
import matplotlib.image as image
from scipy import ndimage
from matplotlib.offsetbox import OffsetImage, AnnotationBbox
fn = get_sample_data(os.getcwd()+"/data/img/exoter.png", asfileobj=False)
exoter = image.imread(fn)
exoter = ndimage.rotate(exoter, -110)
imexoter = OffsetImage(exoter, zoom=0.3)
ab = AnnotationBbox(imexoter, xy=(x[2], y[2]),
xybox=None,
xycoords='data',
boxcoords="offset points",
frameon=False)
ax.annotate(r'ExoTeR', xy=(x[2], y[2]), xycoords='data',
xytext=(-20, -35), textcoords='offset points',
fontsize=30,
#arrowprops=dict(arrowstyle="->", connectionstyle="arc3,rad=.2", lw=2.0)
zorder=101,
)
ax.annotate(r'Start', xy=(x[2], y[2]), xycoords='data',
xytext=(-5, 5), textcoords='offset points',
fontsize=30,
horizontalalignment='left',
verticalalignment='bottom',
zorder=101
)
ax.scatter(x[2], y[2], marker='o', facecolor='k', s=100, alpha=1.0, zorder=103)
#ax.arrow(x[0], y[0], x[130]-x[0], y[130]-y[0], width=0.02, head_width=0.07,
# head_length=0.1, fc='k', ec='k', zorder=104)
# End sign
ax.annotate(r'End', xy=(fr_x[fr_x.shape[0]-1], fr_y[fr_y.shape[0]-1]), xycoords='data',
xytext=(-5, 5), textcoords='offset points',
fontsize=30,
horizontalalignment='left',
verticalalignment='bottom',
zorder=101
)
ax.scatter(fr_x[fr_x.shape[0]-1], fr_y[fr_y.shape[0]-1], marker='o',
facecolor='k', s=100, alpha=1.0, zorder=103)
ax.add_artist(ab)
plt.xlabel(r'X [$m$]', fontsize=35, fontweight='bold')
plt.ylabel(r'Y [$m$]', fontsize=35, fontweight='bold')
ax.legend(loc=2, prop={'size':30})
#plt.axis('equal')
plt.grid(True)
fig.savefig("decos_adaptive_slam_dem_20140911-1805.png", dpi=fig.dpi)
plt.show(block=True)
skidodoPos.readData('../data/20131022_motocross_field/20131022-1812/20131207-1929/data/skid_odometry_position.0.data', cov=True)
skidodoPos.eigenValues()
# GPS/Vicon reference from Ground Truth
refPos = data.ThreeData()
refPos.readData('../data/20131022_motocross_field/20131022-1812/20131207-1929/data/reference_position.0.data', cov=False)
refPos.eigenValues()
#Odometry , Skid Odometry and GPS values(X-Y Axis Motocross Field)
matplotlib.rcParams.update({'font.size': 30, 'font.weight': 'bold'})
fig = plt.figure(1)
ax = fig.add_subplot(111)
plt.rc('text', usetex=False)# activate latex text rendering
rot = quat.quaternion([0.99, 0.0, -0.0087, 0.00])
M = rot.toMatrix()
xposition=[]
yposition=[]
for i in range(0,len(odoPos.data)):
x = odoPos.data[i][0]
y = odoPos.data[i][1]
z = odoPos.data[i][2]
vec = dot(M,[x,y,z])
xposition.append(vec[0])
yposition.append(vec[1])
xposition = xposition[0::80]
yposition = yposition[0::80]
ax.plot(xposition, yposition, marker='o', linestyle='-.', label= "Weighted Jacobian Odometry", color=[0.0,0.8,0], alpha=0.5, lw=2)
ododynpos.append(np.array(odoPosDyn.getAxis(0)))
ododynpos.append(np.array(odoPosDyn.getAxis(1)))
ododynpos.append(np.array(odoPosDyn.getAxis(2)))
skidodoPos = data.ThreeData()
skidodoPos.readData('/home/jhidalgocarrio/esa-npi/dev/bundles/asguard/logs/20131206-2344/data/skid_odometry_position.0.data', cov=True)
skidodoPos.eigenValues()
skidpos=[]
skidpos.append(np.array(skidodoPos.getAxis(0)))
skidpos.append(np.array(skidodoPos.getAxis(1)))
skidpos.append(np.array(skidodoPos.getAxis(2)))
refPos = data.ThreeData()
refPos.readData('/home/jhidalgocarrio/esa-npi/dev/bundles/asguard/logs/20131206-2344/data/reference_position.0.data', cov=False)
refPos.eigenValues()
rot = quat.quaternion([0.819, -0.014, 0.01001, -0.5735]) #Align reference trajectory
M = rot.toMatrix()
xrefpos = []
yrefpos = []
zrefpos = []
for i in range(0,len(refPos.data)):
x = refPos.data[i][0]
y = refPos.data[i][1]
z = refPos.data[i][2]
vec = dot(M,[x,y,z])
xrefpos.append(vec[0])
yrefpos.append(vec[1])
zrefpos.append(vec[2])
refpos=[]
refpos.append(np.array(xrefpos))
# Odometry Pose
pureodoPos = data.ThreeData()
pureodoPos.readData('~/npi/data/20130415_asguard_test_track/20131206-2243/data/odometry_position.0.data', cov=True)
pureodoPos.eigenValues()
# Skid Odometry Pose
skidodoPos = data.ThreeData()
skidodoPos.readData('~/npi/data/20130415_asguard_test_track/20131206-2159/data/skid_odometry_position.0.data', cov=True)
skidodoPos.eigenValues()
#Odometry , Skid Odometry and GPS values(X-Y Axis Sand Field)
matplotlib.rcParams.update({'font.size': 30, 'font.weight': 'bold'})
fig = plt.figure(1, figsize=(28, 16), dpi=120, facecolor='w', edgecolor='k')
ax = fig.add_subplot(111)
rot = quat.quaternion([0.99, 0.00, 0.0, 0.0261])
M = rot.toMatrix()
xposition = []
yposition = []
for i in range(0,len(odoPos.data)):
x = odoPos.data[i][0]
y = odoPos.data[i][1]
z = odoPos.data[i][2]
vec = dot(M,[x,y,z])
xposition.append(vec[0])
yposition.append(vec[1])
xposition = xposition[0::50]
yposition = yposition[0::50]
#!/usr/bin/env python from __future__ import print_function, division, absolute_import import sys import numpy as np import quaternion from numpy import * eps = np.finfo(float).eps rot_mat_eps = 200*eps R1 = np.array([quaternion.quaternion(0, -math.sqrt(0.5), 0, -math.sqrt(0.5))]) print(R1) print(quaternion.as_rotation_matrix(R1)) R2 = quaternion.from_rotation_matrix(quaternion.as_rotation_matrix(R1)) print(R2) d = quaternion.rotation_intrinsic_distance(R1[0], R2[0]) print(d) print() sys.stdout.flush() sys.stderr.flush() assert d < rot_mat_eps, (R1, R2, d) # Can't use allclose here; we don't care about rotor sign
bag = rosbag.Bag(os.path.expanduser(path_reference_position_file))
pose_time = []
pose_pos = []
pose_orient = []
for topic, msg, t in bag.read_messages(topics=['/dory/odom']):
print "-----"
print topic
print "-----"
print msg
print "-----"
print t
pose_time.append(t.to_time())
position = msg.pose.pose.position
orientation = msg.pose.pose.orientation
pose_pos.append(np.array([position.x, position.y, position.z]))
pose_orient.append(quat.quaternion([orientation.w, orientation.x, orientation.y, orientation.z]))
pose_time = np.asarray(pose_time)
pose_pos = np.asarray(pose_pos)
april_tag_pos=[]
april_tag_orient=[]
for topic, msg, t in bag.read_messages(topics=['/tf_static']):
if msg.transforms[0].child_frame_id == 'april_tag_link':
print "-----"
print msg
print "-----"
print t
april_tag_pos = np.array([msg.transforms[0].transform.translation.x,
msg.transforms[0].transform.translation.y,
msg.transforms[0].transform.translation.z])
def test_quaternion_power(Qs):
import math
qpower_precision = 4*eps
# Test equivalence between scalar and real-quaternion exponentiation
for b in [0, 0.0, 1, 1.0, 2, 2.0, 5.6]:
for e in [0, 0.0, 1, 1.0, 2, 2.0, 4.5]:
be = np.quaternion(b**e, 0, 0, 0)
assert allclose(be, np.quaternion(b, 0, 0, 0)**np.quaternion(e, 0, 0, 0), rtol=qpower_precision)
assert allclose(be, b**np.quaternion(e, 0, 0, 0), rtol=qpower_precision)
assert allclose(be, np.quaternion(b, 0, 0, 0)**e, rtol=qpower_precision)
for q in [-3*quaternion.one, -2*quaternion.one, -quaternion.one, quaternion.zero, quaternion.one, 3*quaternion.one]:
for s in [-3, -2.3, -1.2, -1.0, 1.0, 1, 1.2, 2.3, 3]:
for t in [-3, -2.3, -1.2, -1.0, 1.0, 1, 1.2, 2.3, 3]:
assert allclose((s*t)**q, (s**q)*(t**q), rtol=2*qpower_precision)
# Test basic integer-exponent and additive-exponent properties
for q in Qs[Qs_finitenonzero]:
assert allclose(q ** 0, np.quaternion(1, 0, 0, 0), rtol=qpower_precision)
assert allclose(q ** 0.0, np.quaternion(1, 0, 0, 0), rtol=qpower_precision)
assert allclose(q ** np.quaternion(0, 0, 0, 0), np.quaternion(1, 0, 0, 0), rtol=qpower_precision)
assert allclose(((q ** 0.5) * (q ** 0.5)), q, rtol=qpower_precision)
assert allclose(q ** 1.0, q, rtol=qpower_precision)
assert allclose(q ** 1, q, rtol=qpower_precision)
assert allclose(q ** np.quaternion(1, 0, 0, 0), q, rtol=qpower_precision)
assert allclose(q ** 2.0, q * q, rtol=qpower_precision)
assert allclose(q ** 2, q * q, rtol=qpower_precision)
assert allclose(q ** np.quaternion(2, 0, 0, 0), q * q, rtol=qpower_precision)
assert allclose(q ** 3, q * q * q, rtol=qpower_precision)
assert allclose(q ** -1, q.inverse(), rtol=qpower_precision)
assert allclose(q ** -1.0, q.inverse(), rtol=qpower_precision)
for s in [-3, -2.3, -1.2, -1.0, 1.0, 1, 1.2, 2.3, 3]:
for t in [-3, -2.3, -1.2, -1.0, 1.0, 1, 1.2, 2.3, 3]:
assert allclose(q**(s+t), (q**s)*(q**t), rtol=2*qpower_precision)
assert allclose(q**(s-t), (q**s)/(q**t), rtol=2*qpower_precision)
# Check that exp(q) is the same as e**q
for q in Qs[Qs_finitenonzero]:
assert allclose(q.exp(), math.e**q, rtol=qpower_precision)
for s in [0, 0., 1.0, 1, 1.2, 2.3, 3]:
for t in [0, 0., 1.0, 1, 1.2, 2.3, 3]:
assert allclose((s*t)**q, (s**q)*(t**q), rtol=2*qpower_precision)
for s in [1.0, 1, 1.2, 2.3, 3]:
assert allclose(s**q, (q*math.log(s)).exp(), rtol=qpower_precision)
qinverse_precision = 2*eps
for q in Qs[Qs_finitenonzero]:
assert allclose((q ** -1.0) * q, Qs[q_1], rtol=qinverse_precision)
for q in Qs[Qs_finitenonzero]:
assert allclose((q ** -1) * q, Qs[q_1], rtol=qinverse_precision)
for q in Qs[Qs_finitenonzero]:
assert allclose((q ** Qs[q_1]), q, rtol=qpower_precision)
strict_assert(False) # Try more edge cases
for q in [quaternion.x, quaternion.y, quaternion.z]:
assert allclose(quaternion.quaternion(math.exp(-math.pi/2), 0, 0, 0),
q**q, rtol=qpower_precision)
assert allclose(quaternion.quaternion(math.cos(math.pi/2), 0, 0, math.sin(math.pi/2)),
quaternion.x**quaternion.y, rtol=qpower_precision)
assert allclose(quaternion.quaternion(math.cos(math.pi/2), 0, -math.sin(math.pi/2), 0),
quaternion.x**quaternion.z, rtol=qpower_precision)
assert allclose(quaternion.quaternion(math.cos(math.pi/2), 0, 0, -math.sin(math.pi/2)),
quaternion.y**quaternion.x, rtol=qpower_precision)
assert allclose(quaternion.quaternion(math.cos(math.pi/2), math.sin(math.pi/2), 0, 0),
quaternion.y**quaternion.z, rtol=qpower_precision)
assert allclose(quaternion.quaternion(math.cos(math.pi/2), 0, math.sin(math.pi/2), 0),
quaternion.z**quaternion.x, rtol=qpower_precision)
assert allclose(quaternion.quaternion(math.cos(math.pi/2), -math.sin(math.pi/2), 0, 0),
quaternion.z**quaternion.y, rtol=qpower_precision)
def Qs():
q_nan1 = quaternion.quaternion(np.nan, 0., 0., 0.)
q_inf1 = quaternion.quaternion(np.inf, 0., 0., 0.)
q_minf1 = quaternion.quaternion(-np.inf, 0., 0., 0.)
q_0 = quaternion.quaternion(0., 0., 0., 0.)
q_1 = quaternion.quaternion(1., 0., 0., 0.)
x = quaternion.quaternion(0., 1., 0., 0.)
y = quaternion.quaternion(0., 0., 1., 0.)
z = quaternion.quaternion(0., 0., 0., 1.)
Q = quaternion.quaternion(1.1, 2.2, 3.3, 4.4)
Qneg = quaternion.quaternion(-1.1, -2.2, -3.3, -4.4)
Qbar = quaternion.quaternion(1.1, -2.2, -3.3, -4.4)
Qnormalized = quaternion.quaternion(0.18257418583505539, 0.36514837167011077,
0.54772255750516607, 0.73029674334022154)
Qlog = quaternion.quaternion(1.7959088706354, 0.515190292664085,
0.772785438996128, 1.03038058532817)
Qexp = quaternion.quaternion(2.81211398529184, -0.392521193481878,
-0.588781790222817, -0.785042386963756)
return np.array([q_nan1, q_inf1, q_minf1, q_0, q_1, x, y, z, Q, Qneg, Qbar, Qnormalized, Qlog, Qexp],
dtype=np.quaternion)
ax.zaxis.set_major_formatter(FormatStrFormatter('%.02f'))
ax.grid(True)
ax.legend(prop={'size':25})
plt.ylabel(r'Position [$m$]')
plt.xlabel(r'Position [$s$]')
ax.set_xlim([-3,60])
ax.set_ylim([-10,10])
ax.set_zlim([0,5])
plt.show(block=False)
#Odometry , Skid Odometry and GPS values(X-Y Axis Sand Field)
matplotlib.rcParams.update({'font.size': 25})
plt.figure(1)
rot = quat.quaternion([0.819, -0.014, 0.01001, -0.5735])
M = rot.toMatrix()
xposition = []
yposition = []
for i in range(0,len(refPos.data)):
x = refPos.data[i][0]
y = refPos.data[i][1]
z = refPos.data[i][2]
vec = dot(M,[x,y,z])
xposition.append(vec[0])
yposition.append(vec[1])
plt.plot(xposition, yposition, marker='D', linestyle='--', label="GPS", color=[0.5,0,0], alpha=0.5, lw=2)
plt.scatter(xposition[0], yposition[0], marker='D', color=[0,0.5,0.5], alpha=0.5, lw=20)
plt.scatter(xposition[len(xposition)-1], yposition[len(yposition)-1], marker='D', color=[0.5,0,0.5], alpha=0.5, lw=20)
def decos_odometry_dem_figure(fig_num, dem_file,
threed_odometry_trajectory, skid_odometry_trajectory, reference_trajectory,
mis_orient_threed = quat.quaternion(np.array([1.0,0.0,0.0,0.0])), mis_position_threed = [0.00, 0.00, 0.00],
mis_orient_skid = quat.quaternion(np.array([1.0,0.0,0.0,0.0])), mis_position_skid = [0.00, 0.00, 0.00]):
########################
# Load Terrain DEM
########################
import os
plydata = PlyData.read(open(os.path.expanduser(dem_file)))
vertex = plydata['vertex'].data
[dem_px, dem_py, dem_pz] = (vertex[t] for t in ('x', 'y', 'z'))
# define grid.
npts=100
dem_xi = np.linspace(min(dem_px), max(dem_px), npts)
dem_yi = np.linspace(min(dem_py), max(dem_py), npts)
# grid the data.
dem_zi = griddata(dem_px, dem_py, dem_pz, dem_xi, dem_yi, interp='linear')
#################################################
# Misalignment to the map #
#################################################
map_posi_align = [2.00, 9.00, 0.00]
map_orient_align = quat.quaternion.fromAngleAxis(-20.0 * np.pi/180.0, [0.0, 0.0,1.0])
matplotlib.rcParams.update({'font.size': 30, 'font.weight': 'bold'})
fig = plt.figure(fig_num, figsize=(28, 16), dpi=120, facecolor='w', edgecolor='k')
ax = fig.add_subplot(111)
#fig, ax = plt.subplots()
# Display the DEM
plt.rc('text', usetex=False)# activate latex text rendering
CS = plt.contour(dem_xi, dem_yi, dem_zi, 20, linewidths=0.5, colors='k')
CS = plt.contourf(dem_xi, dem_yi, dem_zi, 20, cmap=plt.cm.gray, vmax=abs(dem_zi).max(), vmin=-abs(dem_zi).max())
#plt.clabel(CS, inline=1, fontsize=10) # number in the contour line
# plot data points.
plt.xlim(min(dem_px), max(dem_xi))
plt.ylim(min(dem_py), max(dem_yi))
# Color bar of the dem
cbar = plt.colorbar() # draw colorbar
cbar.ax.set_ylabel(r' terrain elevation [$m$]', fontsize=25, fontweight='bold')
# Plot 3d odometry trajectory
t_odo = np.column_stack((threed_odometry_trajectory[:,0][0::10],
threed_odometry_trajectory[:,1][0::10],
threed_odometry_trajectory[:,2][0::10]))
t_odo[:] = [(mis_orient_threed * i * mis_orient_threed.conj())[1:4] for i in t_odo]
t_odo[:] = [ i + mis_position_threed for i in t_odo ]
t_odo[:] = [(map_orient_align * i * map_orient_align.conj())[1:4] for i in t_odo ]
t_odo[:] = [ i + map_posi_align for i in t_odo ]
x = t_odo[:,0]
y = t_odo[:,1]
ax.plot(x, y, marker='o', linestyle='-.', label="3d odometry", color=[0.3,1.0,0.4], lw=5)
# Plot skid odometry
skid = np.column_stack((skid_odometry_trajectory[:,0][0::10],
skid_odometry_trajectory[:,1][0::10],
skid_odometry_trajectory[:,2][0::10]))
skid[:] = [(mis_orient_skid * i * mis_orient_skid.conj())[1:4] for i in skid]
skid[:] = [ i + mis_position_skid for i in skid ]
skid[:] = [(map_orient_align * i * map_orient_align.conj())[1:4] for i in skid ]
skid[:] = [ i + map_posi_align for i in skid ]
x = skid[:,0]
y = skid[:,1]
ax.plot(x, y, marker='x', linestyle='--', label="skid odometry", color=[0,0.5,1], lw=5)
# Display Ground Truth trajectory
ref = np.column_stack((reference_trajectory[:,0][0::10], reference_trajectory[:,1][0::10], reference_trajectory[:,2][0::10]))
ref[:] = [(map_orient_align * i * map_orient_align.conj())[1:4] for i in ref ]
ref[:] = [ i + map_posi_align for i in ref ]
x = ref[:,0]
y = ref[:,1]
ax.plot(x, y, marker='D', linestyle='--', label="reference trajectory", color=[0.5,0,0], alpha=0.5, lw=5)
import os
from matplotlib.cbook import get_sample_data
from matplotlib._png import read_png
import matplotlib.image as image
from scipy import ndimage
from matplotlib.offsetbox import OffsetImage, AnnotationBbox
fn = get_sample_data(os.getcwd()+"/data/img/exoter.png", asfileobj=False)
exoter = image.imread(fn)
exoter = ndimage.rotate(exoter, -120)
imexoter = OffsetImage(exoter, zoom=0.3)
ab = AnnotationBbox(imexoter, xy=(x[0], y[0]),
xybox=None,
xycoords='data',
boxcoords="offset points",
frameon=False)
ax.annotate(r'ExoTeR', xy=(x[0], y[0]), xycoords='data',
xytext=(-30, -40), textcoords='offset points',
fontsize=30,
#arrowprops=dict(arrowstyle="->", connectionstyle="arc3,rad=.2", lw=2.0)
zorder=101
)
ax.annotate(r'Start', xy=(x[0], y[0]), xycoords='data',
xytext=(-5, 5), textcoords='offset points',
fontsize=30,
horizontalalignment='left',
verticalalignment='bottom',
zorder=101
)
ax.scatter(x[0], y[0], marker='o', facecolor='k', s=100, alpha=1.0, zorder=103)
#ax.arrow(x[0], y[0], x[13]-x[0], y[13]-y[0], width=0.02, head_width=0.1,
# head_length=0.05, fc='k', ec='k', zorder=104)
# End sign
ax.annotate(r'End', xy=(x[x.shape[0]-1], y[y.shape[0]-1]), xycoords='data',
xytext=(-5, 5), textcoords='offset points',
fontsize=30,
horizontalalignment='left',
verticalalignment='bottom',
zorder=101
)
ax.scatter(x[x.shape[0]-1], y[y.shape[0]-1], marker='o', facecolor='k', s=100, alpha=1.0, zorder=103)
ax.add_artist(ab)
plt.xlabel(r'X [$m$]', fontsize=35, fontweight='bold')
plt.ylabel(r'Y [$m$]', fontsize=35, fontweight='bold')
ax.legend(loc=2, prop={'size':30})
#leg = ax.legend(loc=1, prop={'size':30}, fancybox=True)
#leg.get_frame().set_alpha(0.5)
#plt.axis('equal')
plt.grid(True)
fig.savefig("decos_odometry_comparison_20140911-1805.pdf", dpi=fig.dpi)
plt.show(block=False)
xposition = odoPos.getAxis(0)[0::50]
yposition = odoPos.getAxis(1)[0::50]
ax.plot(xposition, yposition, marker='o', linestyle='-.', label= "3D odometry", color=[0.0,0.8,0], alpha=0.5, lw=2)
xposition = pureodoPos.getAxis(0)[0::50]
yposition = pureodoPos.getAxis(1)[0::50]
ax.plot(xposition, yposition, marker='x', linestyle='--', label="contact point odometry", color=[0.3,0.2,0.4], alpha=0.5, lw=2)
xposition = skidodoPos.getAxis(0)[0::50]
yposition = skidodoPos.getAxis(1)[0::50]
ax.plot(xposition, yposition, marker='^', linestyle='-', label="skid odometry", color=[0,0.5,1], lw=2)
rot = quat.quaternion([0.819, -0.014, 0.01001, -0.5735])
M = rot.toMatrix()
xposition = []
yposition = []
for i in range(0,len(refPos.data)):
x = refPos.data[i][0]
y = refPos.data[i][1]
z = refPos.data[i][2]
vec = dot(M,[x,y,z])
xposition.append(vec[0])
yposition.append(vec[1])
xposition = xposition[0::50]
yposition = yposition[0::50]
