def test_inverse(self): # quaternion calc from: # https://www.wolframalpha.com/input/?i=quaternion:+0%2B2i-j-3k&lk=3 r = Rotation.from_quaternion([[+2], [-1], [-3], [+0]], ctype=ctypes.c_double) r_inv = r.inverse() e_inv = array_normalize([[-1/7.], [+1/14.], [+3/14.], [0]]) numpy.testing.assert_allclose( r_inv.quaternion(), e_inv, 1e-15 ) r = Rotation.from_quaternion([[+2], [-1], [-3], [+0]], ctype=ctypes.c_float) r_inv = r.inverse() numpy.testing.assert_allclose( r_inv.quaternion(), e_inv, 1e-7 )
def test_inverse(self): # quaternion calc from: # https://www.wolframalpha.com/input/?i=quaternion:+0%2B2i-j-3k&lk=3 r = Rotation.from_quaternion([[+2], [-1], [-3], [+0]], ctype=ctypes.c_double) r_inv = r.inverse() numpy.testing.assert_equal( r_inv.quaternion(), [[-1/7.], [+1/14.], [+3/14.], [0]] ) r = Rotation.from_quaternion([[+2], [-1], [-3], [+0]], ctype=ctypes.c_float) r_inv = r.inverse() numpy.testing.assert_equal( r_inv.quaternion(), [[numpy.float32(-1/7.)], [numpy.float32(+1/14.)], [numpy.float32(+3/14.)], [0]] )
def test_compose(self): expected_quat = [[+2], [-1], [-3], [+0]] r_ident_d = Rotation(ctypes.c_double) r_ident_f = Rotation(ctypes.c_float) r_other_d = Rotation.from_quaternion(expected_quat, ctypes.c_double) r_other_f = Rotation.from_quaternion(expected_quat, ctypes.c_float) r_res_d = r_ident_d.compose(r_other_d) nose.tools.assert_is_not(r_other_d, r_res_d) numpy.testing.assert_equal(r_res_d, r_other_d) numpy.testing.assert_equal(r_res_d.quaternion(), expected_quat) r_res_f = r_ident_f.compose(r_other_f) nose.tools.assert_is_not(r_other_f, r_res_f) numpy.testing.assert_equal(r_res_f, r_other_f) numpy.testing.assert_equal(r_res_f.quaternion(), expected_quat) # Should also work with multiply operator r_res_d = r_ident_d * r_other_d nose.tools.assert_is_not(r_other_d, r_res_d) numpy.testing.assert_equal(r_res_d, r_other_d) numpy.testing.assert_equal(r_res_d.quaternion(), expected_quat) r_res_f = r_ident_f * r_other_f nose.tools.assert_is_not(r_other_f, r_res_f) numpy.testing.assert_equal(r_res_f, r_other_f) numpy.testing.assert_equal(r_res_f.quaternion(), expected_quat) # Rotation of non-congruent types should be converted automatically r_res_d = r_ident_d.compose(r_other_f) nose.tools.assert_is_not(r_res_d, r_other_f) numpy.testing.assert_equal(r_res_d.quaternion(), r_other_f.quaternion()) numpy.testing.assert_equal(r_res_d.quaternion(), expected_quat) r_res_f = r_ident_f.compose(r_other_d) nose.tools.assert_is_not(r_res_f, r_other_f) numpy.testing.assert_equal(r_res_f.quaternion(), r_other_f.quaternion()) numpy.testing.assert_equal(r_res_f.quaternion(), expected_quat) r_res_d = r_ident_d * r_other_f nose.tools.assert_is_not(r_res_d, r_other_f) numpy.testing.assert_equal(r_res_d.quaternion(), r_other_f.quaternion()) numpy.testing.assert_equal(r_res_d.quaternion(), expected_quat) r_res_f = r_ident_f * r_other_d nose.tools.assert_is_not(r_res_f, r_other_f) numpy.testing.assert_equal(r_res_f.quaternion(), r_other_f.quaternion()) numpy.testing.assert_equal(r_res_f.quaternion(), expected_quat)
def test_inverse(self): # quaternion calc from: # https://www.wolframalpha.com/input/?i=quaternion:+0%2B2i-j-3k&lk=3 r = Rotation.from_quaternion([+2, -1, -3, +0], ctype='d') r_inv = r.inverse() e_inv = array_normalize([-1 / 7., +1 / 14., +3 / 14., 0]) numpy.testing.assert_allclose(r_inv.quaternion(), e_inv, 1e-15) r = Rotation.from_quaternion([+2, -1, -3, +0], ctype='f') r_inv = r.inverse() numpy.testing.assert_allclose(r_inv.quaternion(), e_inv, 1e-7)
def test_inverse(self): # quaternion calc from: # https://www.wolframalpha.com/input/?i=quaternion:+0%2B2i-j-3k&lk=3 r = Rotation.from_quaternion([[+2], [-1], [-3], [+0]], ctype=ctypes.c_double) r_inv = r.inverse() numpy.testing.assert_equal(r_inv.quaternion(), [[-1 / 7.], [+1 / 14.], [+3 / 14.], [0]]) r = Rotation.from_quaternion([[+2], [-1], [-3], [+0]], ctype=ctypes.c_float) r_inv = r.inverse() numpy.testing.assert_equal( r_inv.quaternion(), [[numpy.float32(-1 / 7.)], [numpy.float32(+1 / 14.)], [numpy.float32(+3 / 14.)], [0]])
def test_read_write_krtd_file(self): # Use a random string filename to avoid name collision. fname = 'temp_camera_test_read_write_krtd_file.txt' try: for _ in range(100): c = (rand(3)*2-1)*100 center = EigenArray.from_array([c]) rotation = Rotation.from_quaternion(numpy.random.rand(4)*2-1) intrinsics = CameraIntrinsics(10, (5, 5), 1.2, 0.5, [4, 5, 6]) c1 = Camera(center, rotation, intrinsics) c1.write_krtd_file(fname) c2 = Camera.from_krtd_file(fname) err = numpy.linalg.norm(c1.center-c2.center) assert err < 1e-9, ''.join(['Centers are different by ', str(err)]) c1.rotation.angle_from(c2.rotation) < 1e-12 attr = ['focal_length','aspect_ratio','principle_point','skew', 'dist_coeffs'] for att in attr: v1 = numpy.array(getattr(c1.intrinsics,att)) v2 = numpy.array(getattr(c2.intrinsics,att)) err = numpy.linalg.norm(v1-v2) assert err < 1e-8, ''.join(['Difference ',str(err), ' for attribute: ',att]) finally: if os.path.isfile(fname): os.remove(fname)
def test_from_matrix(self): # Create a non-identity matrix from a different constructor that we # assume works # Create new rotation with that matrix. # New rotation to_matrix method should produce the same matrix pre_r = Rotation.from_quaternion([[+2], [-1], [-3], [+0]]) mat = pre_r.matrix() r = Rotation.from_matrix(mat) numpy.testing.assert_equal(mat, r.matrix())
def test_from_quaternion(self): q = array_normalize([[+2], [-1], [-3], [+0]], float) r = Rotation.from_quaternion(q) numpy.testing.assert_equal( r.quaternion(), q )
def test_from_matrix(self): # Create a non-identity matrix from a different constructor that we # assume works # Create new rotation with that matrix. # New rotation to_matrix method should produce the same matrix pre_r = Rotation.from_quaternion([+2, -1, -3, +0]) mat = pre_r.matrix() r = Rotation.from_matrix(mat) numpy.testing.assert_allclose(mat, r.matrix(), 1e-15)
def test_from_quaternion(self): q_list = [[+2], [-1], [-3], [+0]] r = Rotation.from_quaternion(q_list) numpy.testing.assert_equal( r.quaternion(), q_list )
def test_inverse(self): # quaternion calc from: # https://www.wolframalpha.com/input/?i=quaternion:+0%2B2i-j-3k&lk=3 r = Rotation.from_quaternion([+2, -1, -3, +0], ctype='d') r_inv = r.inverse() e_inv = array_normalize([-1/7., +1/14., +3/14., 0]) numpy.testing.assert_allclose( r_inv.quaternion(), e_inv, 1e-15 ) r = Rotation.from_quaternion([+2, -1, -3, +0], ctype='f') r_inv = r.inverse() numpy.testing.assert_allclose( r_inv.quaternion(), e_inv, 1e-7 )
def test_eq(self): # Identities should equal r1 = Rotation('d') r2 = Rotation('d') nose.tools.assert_equal(r1, r2) r1 = Rotation('f') r2 = Rotation('f') nose.tools.assert_equal(r1, r2) r1 = Rotation('d') r2 = Rotation('f') # r2 should get converted into a double instance for checking nose.tools.assert_equal(r1, r2) r1 = Rotation.from_quaternion([1,2,3,4], ctype='d') r2 = Rotation.from_quaternion([1,2,3,4], ctype='d') nose.tools.assert_equal(r1, r2) r1 = Rotation.from_quaternion([1,2,3,4], ctype='d') r2 = Rotation.from_quaternion([-1,-2,-3,-4], ctype='d') assert r1.angle_from(r2) < 1e-12
def test_eq(self): # Identities should equal r1 = Rotation('d') r2 = Rotation('d') nose.tools.assert_equal(r1, r2) r1 = Rotation('f') r2 = Rotation('f') nose.tools.assert_equal(r1, r2) r1 = Rotation('d') r2 = Rotation('f') # r2 should get converted into a double instance for checking nose.tools.assert_equal(r1, r2) r1 = Rotation.from_quaternion([1, 2, 3, 4], ctype='d') r2 = Rotation.from_quaternion([1, 2, 3, 4], ctype='d') nose.tools.assert_equal(r1, r2) r1 = Rotation.from_quaternion([1, 2, 3, 4], ctype='d') r2 = Rotation.from_quaternion([-1, -2, -3, -4], ctype='d') assert r1.angle_from(r2) < 1e-12
def _new(self, s, r, t): """ :type s: float :type r: Rotation | None :type t: collections.Iterable[float] | None """ # noinspection PyProtectedMember if r is None: r = Rotation(self._ctype) elif r._ctype != self._ctype: # Create new Rotation sharing our type r = Rotation.from_quaternion(r.quaternion(), self._ctype) if t is None: t = EigenArray.from_iterable((0, 0, 0), self._ctype) else: t = EigenArray.from_iterable(t, self._ctype, (3, 1)) return self._call_cfunc('vital_similarity_%s_new' % self._tchar, [ self._ctype, Rotation.c_ptr_type(self._ctype), EigenArray.c_ptr_type(3, 1, self._ctype) ], [s, r, t], self.C_TYPE_PTR)
def test_read_write_krtd_file(self): # Use a random string filename to avoid name collision. fname = 'temp_camera_test_read_write_krtd_file.txt' try: for _ in range(100): c = (rand(3) * 2 - 1) * 100 center = EigenArray.from_array([c]) rotation = Rotation.from_quaternion( numpy.random.rand(4) * 2 - 1) intrinsics = CameraIntrinsics(10, (5, 5), 1.2, 0.5, [4, 5, 6]) c1 = Camera(center, rotation, intrinsics) c1.write_krtd_file(fname) c2 = Camera.from_krtd_file(fname) err = numpy.linalg.norm(c1.center - c2.center) assert err < 1e-9, ''.join( ['Centers are different by ', str(err)]) c1.rotation.angle_from(c2.rotation) < 1e-12 attr = [ 'focal_length', 'aspect_ratio', 'principle_point', 'skew', 'dist_coeffs' ] for att in attr: v1 = numpy.array(getattr(c1.intrinsics, att)) v2 = numpy.array(getattr(c2.intrinsics, att)) err = numpy.linalg.norm(v1 - v2) assert err < 1e-8, ''.join( ['Difference ', str(err), ' for attribute: ', att]) finally: if os.path.isfile(fname): os.remove(fname)
def test_from_quaternion(self): q_list = [[+2], [-1], [-3], [+0]] r = Rotation.from_quaternion(q_list) numpy.testing.assert_equal(r.quaternion(), q_list)
def test_compose(self): # Normalize quaternaion vector. expected_quat = array_normalize([+2., -1., -3., +0.]) r_ident_d = Rotation('d') r_ident_f = Rotation('f') r_other_d = Rotation.from_quaternion(expected_quat, 'd') r_other_f = Rotation.from_quaternion(expected_quat, 'f') r_res_d = r_ident_d.compose(r_other_d) nose.tools.assert_is_not(r_other_d, r_res_d) numpy.testing.assert_equal(r_res_d, r_other_d) numpy.testing.assert_equal(r_res_d.quaternion(), expected_quat) r_res_f = r_ident_f.compose(r_other_f) nose.tools.assert_is_not(r_other_f, r_res_f) numpy.testing.assert_equal(r_res_f, r_other_f) numpy.testing.assert_allclose(r_res_f.quaternion(), expected_quat, 1e-7) # Should also work with multiply operator r_res_d = r_ident_d * r_other_d nose.tools.assert_is_not(r_other_d, r_res_d) numpy.testing.assert_equal(r_res_d, r_other_d) numpy.testing.assert_equal(r_res_d.quaternion(), expected_quat) r_res_f = r_ident_f * r_other_f nose.tools.assert_is_not(r_other_f, r_res_f) numpy.testing.assert_equal(r_res_f, r_other_f) numpy.testing.assert_allclose(r_res_f.quaternion(), expected_quat, 1e-7) # Rotation of non-congruent types should be converted automatically r_res_d = r_ident_d.compose(r_other_f) nose.tools.assert_is_not(r_res_d, r_other_f) numpy.testing.assert_allclose(r_res_d.quaternion(), r_other_f.quaternion(), 1e-7) numpy.testing.assert_allclose(r_res_d.quaternion(), expected_quat, 1e-7) r_res_f = r_ident_f.compose(r_other_d) nose.tools.assert_is_not(r_res_f, r_other_f) numpy.testing.assert_allclose(r_res_f.quaternion(), r_other_f.quaternion(), 1e-7) numpy.testing.assert_allclose(r_res_f.quaternion(), expected_quat, 1e-7) # Equality check between types should pass due to integrety resolution # inside function. r_res_d = r_ident_d * r_other_f nose.tools.assert_is_not(r_res_d, r_other_f) numpy.testing.assert_allclose(r_res_d.quaternion(), r_other_f.quaternion(), 1e-7) numpy.testing.assert_allclose(r_res_d.quaternion(), expected_quat, 1e-7) r_res_f = r_ident_f * r_other_d nose.tools.assert_is_not(r_res_f, r_other_f) numpy.testing.assert_equal(r_res_f.quaternion(), r_other_f.quaternion()) numpy.testing.assert_allclose(r_res_f.quaternion(), expected_quat, 1e-7)
def test_from_quaternion(self): q = array_normalize([+2, -1, -3, +0], float) r = Rotation.from_quaternion(q) numpy.testing.assert_equal( r.quaternion(), q )
def test_from_quaternion(self): q = array_normalize([+2, -1, -3, +0], float) r = Rotation.from_quaternion(q) numpy.testing.assert_equal(r.quaternion(), q)
def convert_pos2krtd_dir(frame_pattern, sussex_dir, pos_dir): """ The POS data format stores each frame's worth of metadata in its own text file with the same name as the frame. The data is stored in the following order: Element Data Type Description 0 double Sensor yaw angle (degrees) 1 double Sensor pitch angle (degrees) 2 double Sensor roll angle (degrees) 3 double Sensor latitude in (decimal degrees) 4 double Sensor longitude in (decimal degrees) 5 double Sensor altitude (feet) 6 double GPS time - seconds in week 7 int GPS time - week of year 8 double Velocity in the North direction (m/s) 9 double Velocity in the East direction (m/s) 10 double Velocity in the up direction (m/s) 11 int IMU status 12 int Local adjustment 13 int Flags ? Arguements: frame_dir : str Global pattern for image files. sussex_dir : str Directory for the Sussex MUTC metadata csv. pos_dir : str Directory for the output POS files. If it does not currently exist, it will be created. Example frame_pattern = '/media/sf_Matt/data/24_Sept_2015_WAMI_Flight_1/frames/*.jpg' sussex_dir = '/media/sf_Matt/data/24_Sept_2015_WAMI_Flight_1/novatel_ins_data_2015-09-24-10-01-18' pos_dir = '/media/sf_Matt/data/24_Sept_2015_WAMI_Flight_1/pos' """ # Frind frames frame_fnames = glob.glob(frame_pattern) # Extract the frame number frame_index = np.zeros(len(frame_fnames), dtype=np.int) for i in range(len(frame_fnames)): fnamei = os.path.split(frame_fnames[i])[1] frame_index[i] = int(fnamei[fnamei.find('frame')+5:fnamei.find('_')]) # -------------------- Parse RawImageTopic.csv --------------------------- fname = glob.glob(''.join([sussex_dir,'/*RawImageTopic.csv']))[0] raw_image_topic_df = pd.read_csv(fname) column_names = ['time','GPSWeek','GPSTime','FrameNumber','ImageHeight', 'ImageWidth','BitDepth','BufferNumber'] raw_image_topic_df.columns = column_names image_height = np.unique(raw_image_topic_df.ImageHeight.as_matrix()) assert len(image_height) == 1, 'ImageHeight must be unique.' image_height = image_height[0] image_width = np.unique(raw_image_topic_df.ImageWidth.as_matrix()) assert len(image_width) == 1, 'ImageWidth must be unique.' image_width = image_width[0] bit_depth = np.unique(raw_image_topic_df.BitDepth.as_matrix()) assert len(bit_depth) == 1, 'BitDepth must be unique.' bit_depth = bit_depth[0] # Define mapping from frame number to UTC time frame_times = dict(zip(raw_image_topic_df.FrameNumber.tolist(), raw_image_topic_df.time.tolist())) # Define mapping from frame number to gps_week gps_week = dict(zip(raw_image_topic_df.FrameNumber.tolist(), raw_image_topic_df.GPSWeek.tolist())) # Define mapping from frame number to gps_sec gps_sec = dict(zip(raw_image_topic_df.FrameNumber.tolist(), raw_image_topic_df.GPSTime.tolist())) # ------------------------------------------------------------------------ # --------------------------- Parse pva.csv ------------------------------ fname = glob.glob(''.join([sussex_dir,'/*mark_1_pva.csv']))[0] pva_df = pd.read_csv(fname) pva_df.columns = ['time','seq','stamp','frame_id','lat','lon','height', 'vel_n','vel_e','vel_u','roll','pitch','azimuth'] # Create interpolation objects against time. time = pva_df.time.as_matrix() # Remove large offset so that the interpolation is better behaved. t0 = time[0] time = time - t0 # Latitude (degrees) get_lat = interp1d(time, pva_df.lat.tolist()) # Longitude (degrees) get_lon = interp1d(time, pva_df.lon.tolist()) # Height above WGS84 ellispiod (meters) get_height = interp1d(time, pva_df.height.tolist()) # yaw/pitch/roll in that order (degrees) get_roll = interp1d(time, pva_df.roll.tolist()) get_pitch = interp1d(time, pva_df.pitch.tolist()) get_azimuth = interp1d(time, pva_df.azimuth.tolist()) if True: # Show the raw yaw. pitch, and roll. plt.subplot('311') plt.plot(time, pva_df.azimuth.tolist()) plt.subplot('312') plt.plot(time, pva_df.pitch.tolist()) plt.subplot('313') plt.plot(time, pva_df.roll.tolist()) # ------------------------------------------------------------------------ # --------------------------- Parse pos_1.csv ------------------------------ fname = glob.glob(''.join([sussex_dir,'/*pos_1.csv']))[0] pos_1_df = pd.read_csv(fname) pos_1_df.columns = ['time','seq','stamp','frame_id','status','service', 'latitude','longitude','altitude', 'position_covariance0','position_covariance1', 'position_covariance2','position_covariance3', 'position_covariance4','position_covariance5', 'position_covariance6','position_covariance7', 'position_covariance8','position_covariance_type'] # Create interpolation objects against time. time = pos_1_df.time.tolist() if False: # Check that the values in pos_1.csv are consistent with pva.csv. # Remove large offset so that the interpolation is better behaved. t0 = time[0] time = time - t0 print('Maximum difference between pos_1.csv and pva.csv latitude', np.max(np.abs(get_lat(time) - pos_1_df.latitude.tolist()))) print('Maximum difference between pos_1.csv and pva.csv longitude', np.max(np.abs(get_lon(time) - pos_1_df.longitude.tolist()))) print('Maximum difference between pos_1.csv and pva.csv altitude', np.max(np.abs(get_height(time) - pos_1_df.altitude.tolist()))) # ------------------------------------------------------------------------ try: os.mkdir(pos_dir) except: pass # Write out one POS txt file per image frame. for i in range(len(frame_fnames)): # The POS file name will match that of the image frame. img_fname = os.path.splitext(os.path.split(frame_fnames[i])[1])[0] pos_fname = ''.join([pos_dir,'/',img_fname,'.pos']) pos_datai = np.zeros(14) # Frame time in UTC nanoseconds. timei = frame_times[frame_index[i]] # ---------------- Determine Orientation of Camera ------------------- # yaw/pitch/roll of the INS. ins_yaw = get_azimuth(timei-t0) # (degrees) ins_pitch = get_pitch(timei-t0) # (degrees) ins_roll = get_roll(timei-t0) # (degrees) # Rotation operator that moves the INS coordinate system from aligned # with the NED axes to its current state. ins_rot = Rotation.from_quaternion(quat_from_ypr(ins_yaw, ins_pitch, ins_roll, in_degrees=True)) # Hard-coded camera orientation calibration. This rotates the INS into # the Airforce's camera coordinate system, which is non-standard. ins_to_cam_prime = Rotation.from_quaternion(quat_from_ypr(91.625, .867, -.612, in_degrees=True)) # The AF camera has the look direction as x, the y going out the right, # and z going down. We need to rotate this into x-right, y-down, # z-look direction. cp_to_c = Rotation.from_quaternion(quat_from_ypr(90, 0, 90, in_degrees=True)) rot_new = cp_to_c*ins_to_cam_prime*ins_rot yaw, pitch, roll = ypr_from_quat(rot_new.quaternion(), in_degrees=True) # Sensor yaw in (degrees) pos_datai[0] = yaw # Sensor pitch in (degrees) pos_datai[1] = pitch # Sensor roll (degrees) pos_datai[2] = roll # -------------------------------------------------------------------- # Sensor latitude in (decimal degrees) pos_datai[3] = get_lat(timei-t0) # Sensor longitude in (decimal degrees) pos_datai[4] = get_lon(timei-t0) # Sensor altitude (feet above WGS84) pos_datai[5] = get_height(timei-t0)*3.28084 # GPS time - seconds in week pos_datai[6] = gps_sec[frame_index[i]] # Validate against the image filename err = int(img_fname.split('_')[1]) - gps_sec[frame_index[i]] assert np.abs(err) < 1, 'GPS seconds in image filename does not match.' # GPS time - week of year pos_datai[7] = gps_week[frame_index[i]] # Velocity in the North direction (m/s) pos_datai[8] = 0 # Velocity in the East direction (m/s) pos_datai[9] = 0 # Velocity in the up direction (m/s) pos_datai[10] = 0 # IMU status pos_datai[11] = -1 # NOT SURE WHAT THIS DOES # Local adjustment pos_datai[12] = 0 # NOT SURE WHAT THIS DOES # Flags ? pos_datai[13] = 0 # NOT SURE WHAT THIS DOES np.savetxt(pos_fname, np.atleast_2d(pos_datai), delimiter=',', fmt='%.10f,'*7 + '%i,' + '%.10f,'*3 + '%i,'*2 + '%i')