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