def check_convergence(previous_pose, predicted_pose):
prevT, predT, identityT = np.zeros((4, 4)), np.zeros((4, 4)), np.zeros(
(4, 4))
prevT[3, 3], predT[3, 3] = 1, 1
identityT[0, 0], identityT[1, 1], identityT[2,
2], identityT[3,
3] = 1, 1, 1, 1
prevT[0:3, 3] = previous_pose[0:3]
predT[0:3, 3] = predicted_pose[0:3]
prevT[0:3, 0:3] = t3d.quat2mat([
previous_pose[3], previous_pose[4], previous_pose[5], previous_pose[6]
])
predT[0:3, 0:3] = t3d.quat2mat([
predicted_pose[3], predicted_pose[4], predicted_pose[5],
predicted_pose[6]
])
errorT = np.dot(predT, np.linalg.inv(prevT))
errorT = errorT - identityT
errorT = errorT * errorT
error = np.sum(errorT)
converged = False
if error < FLAGS.threshold:
converged = True
return converged
def test_pose_utils():
"""
Tests the pose utils
:return:
"""
TEST_COMPOSE = True
TEST_INV = True
ra = lambda _: np.random.uniform(0, 2 * math.pi)
if TEST_COMPOSE:
print('Testing pose composing...')
R1 = txe.euler2mat(ra(1), ra(1), ra(1))
t1 = np.random.rand(3)
R2 = txe.euler2mat(ra(1), ra(1), ra(1))
t2 = np.random.rand(3)
# homogeneous matrix method
R = np.dot(R1, R2)
t = t1 + np.dot(R1, t2)
print('From homogeneous matrices:\nt = {}\nR = {}\n'.format(t, R))
# quaternion method
q1 = txq.mat2quat(R1)
q2 = txq.mat2quat(R2)
p1 = torch.cat((torch.from_numpy(t1), torch.from_numpy(q1)))
p2 = torch.cat((torch.from_numpy(t2), torch.from_numpy(q2)))
p = compose_pose_quaternion(torch.unsqueeze(p1, 0),
torch.unsqueeze(p2, 0))
t = p[:, :3].numpy().squeeze()
q = p[:, 3:].numpy().squeeze()
print
'From quaternions, t = '
print
t
print
'R = '
print
txe.quat2mat(q)
if TEST_INV:
print('Testing pose inversion...')
R = txe.euler2mat(ra(1), ra(1), ra(1))
t = np.random.rand(3)
T = np.eye(4)
T[:3, :3] = R
T[:3, -1] = t
q = txq.mat2quat(R)
p = torch.cat((torch.from_numpy(t), torch.from_numpy(q)))
pinv = invert_pose_quaternion(torch.unsqueeze(p, 0))
tinv, qinv = pinv[:, :3], pinv[:, 3:]
Rinv = txq.quat2mat(qinv.numpy().squeeze())
Tinv = np.eye(4)
Tinv[:3, :3] = Rinv
Tinv[:3, -1] = tinv.numpy().squeeze()
print('T * T^(-1) = {}'.format(np.dot(T, Tinv)))
def transformation_quat2mat(poses, TRANSFORMATIONS,
templates_data): # (for 4b)
# Arguments:
# poses: 7D (x,y,z,quat_q0,quat_q1,quat_q2,quat_q3) (in radians) (batch_size x 7)
# TRANSFORMATIONS: Overall tranformation matrix.
# template_data: Point Cloud (batch_size x num_points x 3)
# Output
# TRANSFORMATIONS: Batch_size x 4 x 4
# templates_data: Transformed template data (batch_size x num_points x 3)
poses = np.array(poses) # Convert poses to array.
poses = poses.reshape(poses.shape[-2], poses.shape[-1])
for i in range(poses.shape[0]):
transformation_matrix = np.zeros((4, 4))
transformation_matrix[3, 3] = 1
rot = t3d.quat2mat(
[poses[i, 3], poses[i, 4], poses[i, 5],
poses[i, 6]]) # Calculate rotation matrix using transforms3d
transformation_matrix[
0:3, 0:3] = rot # Store rotation matrix in transformation matrix.
transformation_matrix[0:3, 3] = poses[
i, 0:3] # Store translations in transformation matrix.
TRANSFORMATIONS[i, :, :] = np.dot(
transformation_matrix, TRANSFORMATIONS[i, :, :]
) # 4b (Multiply tranfromation matrix to Initial Transfromation Matrix)
templates_data[i, :, :] = np.dot(
rot,
templates_data[i, :, :].T).T # Apply Rotation to Template Data
templates_data[i, :, :] = templates_data[i, :, :] + poses[
i, 0:3] # Apply translation to template data
return TRANSFORMATIONS, templates_data
def getCorners(self, size, location, quaternion=np.array([1, 0, 0, 0])):
for i in range(2):
for j in range(2):
for k in range(2):
self.vertex[i * 4 + j * 2 + k] = np.array([
location[0] + k * size[0], location[1] + j * size[1],
location[2] + i * size[2]
])
R = eu.quat2mat(quaternion)
vertex = np.array(self.vertex, np.float32)
return np.dot(R, vertex.transpose())
def rotate_box(self, quaternion):
vertex = []
if isinstance(quaternion, np.ndarray):
if np.shape(quaternion) == (4, 1):
self.rotation = quaternion.transpose()
elif np.shape(quaternion) == (1, 4):
self.rotation = quaternion
elif isinstance(quaternion, list):
self.rotation = np.array(quaternion)
for i in range(2):
for j in range(2):
for k in range(2):
vertex.append([k * self.x, j * self.y, i * self.z])
# (0 0 0)(1 0 0)(0 1 0)(1 1 0)(0 0 1)(1 0 1)(0 1 1)(1 1 1)
self.R = eu.quat2mat(quaternion)
vertex = np.array(vertex, np.float32)
vertex = np.dot(self.R, vertex.transpose()).transpose()
for s in vertex:
s += self.location
self.vertex = vertex
self.centre = (self.vertex[7] - self.vertex[0]) / 2 + self.location
Pqt_1_bar, Pcov, Pqt1, e = PM.prediction(
ut_t, E, Rq_imu) #where e is the error vector
#qt+1 bar is the predicted mean, cov is the predicted mean, Pqt1 is the predicted 7 sigma points
#measurement
zt1_bar, cov_vv, zt1 = PM.measurement(Pqt1)
#E_qt1 = PM.sigma(Pcov, 0)
#kalem gain
kt1 = PM.kgain(e, zt1_bar, zt1, cov_vv)
#update
qt1_updated, covt1_updated = PM.update(kt1, acc_imu, zt1_bar, cov_vv,
Pqt_1_bar, Pcov)
covt_t = covt1_updated
ut_t = qt1_updated
# euler_ut_t = quat2euler(ut_t)
# estimate_orienstate.append(euler_ut_t)
rot_matrix = quat2mat(ut_t)
estimate_orienstate.append(rot_matrix)
print(len(estimate_orienstate))
print(cam_values.shape)
idx = qtn.find_nearest(imu_ts[0], cam_ts)
print(idx.shape)
vert, hori, RGB, camsize = cam_values.shape
print(camsize)
Panorama_Field = np.zeros((4 * 240, 6 * 320, 3), dtype=np.uint8)
d3mask = qtn.get_mask(
vert,
hori) # constant mask can be used in multi times, with cartesian value
d2mask = d3mask.transpose(2, 0, 1).reshape(3, -1) # transform from 3d to 2d