def getTransformationFromEulerAnglesRollPitchYawRadXYZMeters( roll_rad, pitch_rad, yaw_rad, x_m, y_m, z_m): q = getQuaternionFromFromEulerAnglesRollPitchYawRad( roll_rad, pitch_rad, yaw_rad) p = np.array([x_m, y_m, z_m]) T = mk.Transformation(q, p) return T
def get_T_B_I(): C_B_I = np.array([[0.0, 0.0, 1.0], [1.0, 0.0, 0.0], [0.0, 1.0, 0.0]]) T_BZU_B = mk.Transformation(mk.Quaternion(C_B_I), np.array([0.0, 0.0, 0.0])) return T_BZU_B
def get_T_B_XB3(): C_BZU_B = np.array([[1.0, 0.0, 0.0], [0.0, -1.0, 0.0], [0.0, 0.0, -1.0]]) T_BZU_B = mk.Transformation(mk.Quaternion(C_BZU_B), np.array([0.0, 0.0, 0.0])) return T_BZU_B
def parse_synchronized_odometry(odometry_file, odometry_cov_file): assert (os.path.exists(odometry_file)) assert (os.path.exists(odometry_cov_file)) odom = np.loadtxt(odometry_file, delimiter=",") odom_cov = np.loadtxt(odometry_cov_file, delimiter=",") num_measurements = len(odom) assert (num_measurements == len(odom_cov)) T_O_BZUkm1 = mk.Transformation() time_us_T_O_BZUks_with_covs = [] T_BZU_B = get_T_BZU_B() print 'Parsing odometry...' for i in range(num_measurements): utime = odom[i, 0] x_m = odom[i, 1] y_m = odom[i, 2] z_m = odom[i, 3] roll_rad = odom[i, 4] pitch_rad = odom[i, 5] yaw_rad = odom[i, 6] T_Bkm1_Bk = getTransformationFromEulerAnglesRollPitchYawRadXYZMeters( roll_rad, pitch_rad, yaw_rad, x_m, y_m, z_m) T_BZUkm1_BZUk = T_BZU_B * T_Bkm1_Bk * T_BZU_B.inverse() T_O_BZUk = T_O_BZUkm1 * T_BZUkm1_BZUk cov = np.array([[ odom_cov[i, 1], odom_cov[i, 2], odom_cov[i, 3], odom_cov[i, 4], odom_cov[i, 5], odom_cov[i, 6] ], [ odom_cov[i, 2], odom_cov[i, 7], odom_cov[i, 8], odom_cov[i, 9], odom_cov[i, 10], odom_cov[i, 11] ], [ odom_cov[i, 3], odom_cov[i, 8], odom_cov[i, 12], odom_cov[i, 13], odom_cov[i, 14], odom_cov[i, 15] ], [ odom_cov[i, 4], odom_cov[i, 9], odom_cov[i, 13], odom_cov[i, 16], odom_cov[i, 17], odom_cov[i, 18] ], [ odom_cov[i, 5], odom_cov[i, 10], odom_cov[i, 14], odom_cov[i, 17], odom_cov[i, 19], odom_cov[i, 20] ], [ odom_cov[i, 6], odom_cov[i, 11], odom_cov[i, 15], odom_cov[i, 17], odom_cov[i, 20], odom_cov[i, 21] ]]) time_us_T_O_BZUks_with_covs.append((utime, T_O_BZUk, cov)) T_O_BZUkm1 = T_O_BZUk print 'done' #xs = [T.getPosition()[0] for t, T, c in time_us_T_O_Bks_with_covs] #ys = [T.getPosition()[1] for t, T, c in time_us_T_O_Bks_with_covs] #zs = [T.getPosition()[2] for t, T, c in time_us_T_O_Bks_with_covs] #IPython.embed() return time_us_T_O_BZUks_with_covs