def test_len_and_bool():
rotation_single = Rotation([0, 0, 0, 1])
rotation_multi = Rotation([[0, 0, 0, 1], [0, 0, 0, 1]])
with pytest.raises(TypeError, match="Single rotation has no len()."):
len(rotation_single)
assert len(rotation_multi) == 2
assert rotation_single
assert rotation_multi
def __add__(self, other):
"""
concatenate two set of quaternions
"""
times = np.concatenate([self.times, other.times])
qself = self(self.times) if self.times.size else Rotation(
np.empty((0, 4), np.double))
qother = other(other.times)
quats = np.concatenate([qself.as_quat(), qother.as_quat()])
ts, idx = np.unique(times, return_index=True)
return self.__class__(ts, Rotation(quats[idx]))
def get_rotation_map(self, img, quat):
"""Get maps for doing perspective rotations
WORK IN PROGRESS. Currently for testing
"""
# https://stackoverflow.com/a/12293128
# https://en.wikipedia.org/wiki/Homography_(computer_vision)
rotXval = 0
rotYval = 0
rotZval = 0
rotX = (rotXval) * np.pi / 180
rotY = (rotYval) * np.pi / 180
rotZ = (rotZval) * np.pi / 180
rot_mat = np.eye(4)
#print(Rotation([quat[0,1],quat[0,2],quat[0,3],quat[0,0]]).as_euler('xyz'))
quat = quat.flatten()
eul = Rotation([quat[1], quat[2], quat[3], quat[0]]).as_euler('xyz')
combined_rotation = np.eye(4)
#combined_rotation[0:3,0:3] = Rotation.from_euler('xyz', [eul[0], eul[1], -eul[2]], degrees=False).as_matrix()
combined_rotation[0:3, 0:3] = Rotation(
[-quat[1], -quat[2], quat[3], -quat[0]]).as_matrix()
rot_mat = combined_rotation
img_dim = img.shape[:2][::-1]
# Scaled 3x4 camera matrix
K = np.zeros((3, 4))
K[:3, :3] = self.K
# should make the rotation match fov change
K[0, 0] = self.new_K[0, 0]
K[1, 1] = self.new_K[1, 1]
K *= img_dim[0] / self.calib_dimension[0]
K[2][2] = 1.0
# compute inverse camera matrix using scaled K
Kinv = np.zeros((4, 3))
Kinv[0:3, 0:3] = inverse_cam_mtx(K[:3, :3])
Kinv[3, :] = [0, 0, 1]
H = np.linalg.multi_dot([K, rot_mat, Kinv])
#transform = self.K * trans
outimg = cv2.warpPerspective(img, H, (img.shape[1], img.shape[0]))
return outimg
def make_gyro_relativistic_correction(attdata):
if attdata.times.size == 0:
return attdata
print("inverse relativistic correction required")
vec = attdata(attdata.times).apply(OPAX)
ra, dec = vec_to_pol(vec)
ra, dec = np.rad2deg(ra), np.rad2deg(dec)
sc = SkyCoord(ra,
dec,
unit=("deg", "deg"),
frame="fk5",
obstime=atime.Time(51543.875, format="mjd") +
atime.TimeDelta(attdata.times, format="sec"))
vec2 = np.asarray(sc.gcrs.cartesian.xyz.T)
vrot = np.cross(vec2, vec)
vrot = vrot / np.sqrt(np.sum(vrot**2, axis=1))[:, np.newaxis]
calpha = np.sum(vec * vec2, axis=1)
calphap2 = np.sqrt((calpha + 1.) / 2.)
salphap2 = np.sqrt((1. - calpha) / 2.)
#alpha = np.arccos(np.sum(vec*vec2, axis=1))
qcorr = np.empty((calphap2.size, 4), np.double)
qcorr[:, :3] = vrot * salphap2[:, np.newaxis]
qcorr[:, 3] = calphap2
return AttDATA(attdata.times,
Rotation(qcorr).inv() * attdata(attdata.times),
gti=attdata.gti)
def get_earth_rot_quats(times, t0=None):
"""
for provided times and T0 epoche computes rotation of the
initial plan for this function was to pack wery densly attitude data information
it was assumed that spacecraft rotates around axis, which connects is with the sun,
this axis in turn rotates in solar plane with 1yr period.
Unfortunately, it was found that the axis, arond which SC rotates, does not coincide with the sc-snu axis,
also this axis changes with time, and its rotation speed also changes, so the function is useless for now
-------
Params:
times - times for which compute rotation of frozen fk5 system due to the earth rotation around the sun
return:
quaternions for each provided time point which rotates fk5 frame
"""
if t0 is None: t0 = times[0]
q = np.empty(np.asarray(times).shape + (4, ), np.double)
print(q.shape)
phase = 2. * pi / SECPERYR * (times - t0)
sphase = np.sin(phase / 2.)
q[..., 0] = SOLARSYSTEMPLANENRMALEINFK5[0] * sphase
q[..., 1] = SOLARSYSTEMPLANENRMALEINFK5[1] * sphase
q[..., 2] = SOLARSYSTEMPLANENRMALEINFK5[2] * sphase
q[..., 3] = np.cos(phase / 2.)
return Rotation(q)
def get_maps(self, fov_scale = 1.0, new_img_dim = None, update_new_K = True, quat = None):
"""Get undistortion maps
Args:
fov_scale (float, optional): Virtual camera focal length divider. Defaults to 1.
new_img_dim (tuple, optional): Dimension of new image
Returns:
(np.ndarray,np.ndarray): Undistortion maps
"""
img_dim = new_img_dim if new_img_dim else self.calib_dimension
scaled_K = self.K * img_dim[0] / self.calib_dimension[0]
scaled_K[2][2] = 1.0
new_K = cv2.fisheye.estimateNewCameraMatrixForUndistortRectify(scaled_K, self.D,
img_dim, np.eye(3), fov_scale=fov_scale)
R = np.eye(3)
if type(quat) != type(None):
quat = quat.flatten()
R = Rotation([-quat[1],-quat[2],quat[3],-quat[0]]).as_matrix()
if update_new_K:
self.new_K = new_K
map1, map2 = cv2.fisheye.initUndistortRectifyMap(scaled_K, self.D, R, new_K, img_dim, cv2.CV_16SC2)
return map1, map2
def endpoint_pos(self):
"""Get the end-effector's endpoint position
"""
state = self.p.getLinkState(self.id, self.num_joints-1)
pos = state[0]
z_dir = Rotation(state[1]).as_matrix()[:, -1]
return pos + self.ee_l/2*z_dir
def concatenate(cls, attlist):
qlist = np.concatenate([att(att.times).as_quat() for att in attlist],
axis=0)
tlist = np.concatenate([att.times for att in attlist])
tgti = reduce(lambda a, b: a | b, [att.gti for att in attlist])
ut, uidx = np.unique(tlist, return_index=True)
return cls(ut, Rotation(qlist[uidx]), gti=tgti)
def setUp(self):
self.n1 = 50
self.n3 = 30
self.w = 5
self.t1 = np.random.normal(size=(self.n1, 3))
self.R1 = Rotation(np.random.normal(size=(self.n1, 4)))
self.t2 = np.random.normal(size=(self.n1, 3))
self.R2 = Rotation(np.random.normal(size=(self.n1, 4)))
self.t3 = np.random.normal(size=(self.n3, 3))
self.R3 = Rotation(np.random.normal(size=(self.n3, 4)))
self.t4 = np.random.normal(size=3)
self.R4 = Rotation(np.random.normal(size=4))
self.poses1 = SE3Poses(self.t1, self.R1)
self.poses2 = SE3Poses(self.t2, self.R2)
self.poses3 = SE3Poses(self.t3, self.R3)
self.poses4 = SE3Poses(self.t4, self.R4)
def parse_metadata(metadata):
data = {}
root = ET.fromstring(metadata)
x = float(root.find('position').attrib['x'])
y = float(root.find('position').attrib['y'])
z = float(root.find('position').attrib['z'])
data['position'] = {'x': x, 'y': y, 'z': z}
x = float(root.find('quaternion').attrib['x'])
y = float(root.find('quaternion').attrib['y'])
z = float(root.find('quaternion').attrib['z'])
w = float(root.find('quaternion').attrib['w'])
data['quaternion'] = {'x': x, 'y': y, 'z': z, 'w': w}
data['rotation'] = Rotation((x, y, z, w)).as_euler('zxy')
x_dot = float(root.find('velocity').attrib['x_dot'])
y_dot = float(root.find('velocity').attrib['y_dot'])
z_dot = float(root.find('velocity').attrib['z_dot'])
data['velocity'] = {'x_dot': x_dot, 'y_dot': y_dot, 'z_dot': z_dot}
x_ang_dot = float(root.find('angular_velocity').attrib['x_ang_dot'])
y_ang_dot = float(root.find('angular_velocity').attrib['y_ang_dot'])
z_ang_dot = float(root.find('angular_velocity').attrib['z_ang_dot'])
data['angular_velocity'] = {
'x_ang_dot': x_ang_dot,
'y_ang_dot': y_ang_dot,
'z_ang_dot': z_ang_dot
}
x_ddot = float(root.find('acceleration').attrib['x_ddot'])
y_ddot = float(root.find('acceleration').attrib['y_ddot'])
z_ddot = float(root.find('acceleration').attrib['z_ddot'])
data['acceleration'] = {
'x_ddot': x_ddot,
'y_ddot': y_ddot,
'z_ddot': z_ddot
}
x_ang_ddot = float(root.find('angular_acceleration').attrib['x_ang_ddot'])
y_ang_ddot = float(root.find('angular_acceleration').attrib['y_ang_ddot'])
z_ang_ddot = float(root.find('angular_acceleration').attrib['z_ang_ddot'])
data['angular_acceleration'] = {
'x_ang_ddot': x_ang_ddot,
'y_ang_ddot': y_ang_ddot,
'z_ang_ddot': z_ang_ddot
}
data['time'] = float(root.find('time').text)
data['collision'] = root.find(
'collision').attrib['status'].lower() == 'true'
return data
def make_small_steps_quats(attdata, gti=tGTI, timecorrection=lambda x: 1.):
"""
provided with AttDATA container (see arttools.orientation.AttDATA)
produces a set of quaternions, which separated not more than by DELTASY in angles and DELTAROLL in rolls
params: 'attdata' (AttDATA container, which defines interpolation of quaterninons with time within attached gti)
'timecorrection' - function which produce timescaling depending on time (time weights)
returns: qval, exptime, gti - quatertions, exposure time for this quaternions, and resulted overall gti
"""
locgti = gti & attdata.gti
tnew, maskgaps = locgti.make_tedges(attdata.times)
if tnew.size == 0:
return Rotation(np.empty((0, 4), np.double)), np.array([])
ts = ((tnew[1:] + tnew[:-1]) / 2.)[maskgaps]
dt = (tnew[1:] - tnew[:-1])[maskgaps]
qval = attdata(ts)
ra, dec, roll = quat_to_pol_and_roll(attdata(tnew))
vec = pol_to_vec(ra, dec)
vecprod = np.sum(vec[1:, :] * vec[:-1, :], axis=1)
"""
this ugly thing appears due to the numerical precision
"""
vecprod[vecprod > 1.] = 1.
dalpha = np.arccos(vecprod)[maskgaps]
cs = np.cos(roll)
ss = np.sin(roll)
vecprod = np.minimum(ss[1:] * ss[:-1] + cs[1:] * cs[:-1], 1.)
droll = np.arccos(vecprod)[maskgaps]
maskmoving = (dalpha < DELTASKY) & (droll < DELTAROLL)
qvalstable = qval[maskmoving]
maskstable = np.logical_not(maskmoving)
if np.any(maskstable):
tsm = (ts - dt / 2.)[maskstable]
size = np.maximum(dalpha[maskstable] / DELTASKY,
droll[maskstable] / DELTAROLL).astype(np.int)
dtm = np.repeat(dt[maskstable] / size, size)
ar = np.arange(size.sum()) - np.repeat(
np.cumsum([
0,
] + list(size[:-1])), size) + 0.5
tnew = np.repeat(tsm, size) + ar * dtm
dtn = np.concatenate([
dt[maskmoving], dtm
]) #*timecorrection(ts[maskmoving]), dtm*timecorrection(tnew)])
ts = np.concatenate([ts[maskmoving], tnew])
qval = attdata(ts)
else:
dtn = dt
return ts, qval, dtn * timecorrection(ts), locgti
def parse_metadata(metadata: str) -> AgentState:
""" Get position, orientation, velocity, and acceleration from metadata
Args:
metadata (str): TESSE metadata.
Returns:
AgentState: Object containing position, orientation, velocity, and
acceleration values.
"""
root = ET.fromstring(metadata)
position = Position(*get_attributes(root, "position", ("x", "y", "z")))
x, y, z, w = get_attributes(root, "quaternion", ("x", "y", "z", "w"))
quat = Quaternion(x, y, z, w)
rot = Rot(*Rotation((x, y, z, w)).as_euler("zxy"))
velocity = Velocity(*get_attributes(root, "velocity", ("x_dot", "y_dot", "z_dot")))
angular_velocity = AngularVelocity(
*get_attributes(
root, "angular_velocity", ("x_ang_dot", "y_ang_dot", "z_ang_dot")
)
)
acceleration = Acceleration(
*get_attributes(root, "acceleration", ("x_ddot", "y_ddot", "z_ddot"))
)
angular_acceleration = AngularAcceleration(
*get_attributes(
root, "angular_acceleration", ("x_ang_ddot", "y_ang_ddot", "z_ang_ddot")
)
)
state = AgentState(
position=position,
quaternion=quat,
rotation=rot,
velocity=velocity,
angular_velocity=angular_velocity,
acceleration=acceleration,
angular_acceleration=angular_acceleration,
time=float(root.find("time").text),
collision=root.find("collision").attrib["status"].lower() == "true",
)
return state
def boundingPolygon(self, quat, numPoints=9):
(original_width, original_height) = self.calib_dimension
R = np.eye(3)
if type(quat) != type(None):
quat = quat.flatten()
#R = Rotation([-quat[1],-quat[2],quat[3],-quat[0]]).as_matrix()
R = Rotation([quat[1], quat[2], quat[3], quat[0]]).as_matrix()
R[[0, 0, 1, 2], [1, 2, 0, 0]] *= -1
distorted_points = []
for i in range(numPoints - 1):
distorted_points.append(
(i * (original_width / (numPoints - 1)), 0))
for i in range(numPoints - 1):
distorted_points.append(
(original_width, i * (original_height / (numPoints - 1))))
for i in range(numPoints - 1):
p = numPoints - 1 - i
distorted_points.append(
(p * (original_width / (numPoints - 1)), original_height))
for i in range(numPoints - 1):
p = numPoints - 1 - i
distorted_points.append(
(0, p * (original_height / (numPoints - 1))))
distorted_points = np.array(distorted_points, np.float64)
distorted_points = np.expand_dims(
distorted_points,
axis=0) #add extra dimension so opencv accepts points
undistorted_points = cv2.fisheye.undistortPoints(distorted_points,
self.K,
self.D,
R=R,
P=self.K)
undistorted_points = undistorted_points[
0, :, :] #remove extra dimension
#mtop = np.max(undistorted_points[:(numPoints-1),1])
#mbottom = np.min(undistorted_points[numPoints:(2*numPoints-1),1])
#mleft = np.max(undistorted_points[(2*numPoints):(3*numPoints-1),0])
#mright = np.min(undistorted_points[(3*numPoints):,0])
return undistorted_points
def smooth_orientations_internal(self, times, orientation_list):
alpha = 1
smooth = self.get_user_option_value("smoothness")
if smooth > 0:
alpha = 1 - np.exp(-(1 / self.gyro_sample_rate) / smooth)
smoothed_orientation = np.zeros(orientation_list.shape)
value = orientation_list[0, :]
for i in range(self.num_data_points):
value = quat.single_slerp(value, orientation_list[i, :], alpha)
smoothed_orientation[i] = value
# reverse pass
start_orientations = np.zeros(orientation_list.shape)
value2 = smoothed_orientation[-1, :]
for i in range(self.num_data_points - 1, -1, -1):
value2 = quat.single_slerp(value2, smoothed_orientation[i, :],
alpha)
start_orientations[i] = value2
else:
start_orientations = np.array(orientation_list)
# swap around
start_orientations[:, [0, 1, 2, 3]] = start_orientations[:,
[1, 2, 3, 0]]
eul = Rotation(start_orientations).as_euler("zxy")
#plt.figure()
#plt.plot(eul[:,0])
#plt.plot(eul[:,1])
#plt.plot(eul[:,2])
#plt.show()
eul[:, 0] = self.get_user_option_value("horizon_angle") * np.pi / 180
#new_quat = Rotation.from_euler(["xyz", "zxy", "yzx", "xzy", "zyx", "yxz"][self.get_user_option_value("eul")], eul).as_quat()
new_quat = Rotation.from_euler("zxy", eul).as_quat()
new_quat[:, [0, 1, 2, 3]] = new_quat[:, [3, 0, 1, 2]]
return times, new_quat
def state_deriv(t, x):
c = self.a * self.alpha
ang_vel_mat = np.array([
[ 0, x[6], -x[5], x[4]],
[-x[6], 0, x[4], x[5]],
[ x[5], -x[4], 0, x[6]],
[-x[4], -x[5], -x[6], 0]
])
orientation = Rotation(x[7:11])
net_force = orientation.apply([0, 0, self.alpha * np.sum(x[0:4])])
return np.array([
0, 0, 0, 0, # Motor torque derivatives
(x[1] - x[3])*c/self.principal_moments[0], # Angular accel x
(x[2] - x[0])*c/self.principal_moments[1], # Angular accel y
(x[0] + x[2] - x[1] - x[3])/self.principal_moments[2] # Angular accel z
] + list(0.5 * ang_vel_mat @ orientation.as_quat()) # Quaternion derivative
+ list([0, 0, -9.8] + net_force/self.mass) # Acceleration
)
def align_with_z_quat(vec):
"""
for given vector, provides quaternion, which puts this vector along z axis [0, 0, 1] with shortest possible trajectory
-------
Params:
cartesian vector
returns:
quaternion in the scipy.spatial.transform.Rotation container
"""
vec = vec / sqrt(np.sum(vec**2.))
vrot = np.cross(vec, [0, 0, 1])
vrot = vrot / sqrt(np.sum(vrot**2.))
alpha = np.arccos(vec[2])
q = np.empty(4, np.double)
q[:3] = vrot * sin(alpha / 2.)
q[3] = cos(alpha / 2.)
return Rotation(q)
def _get_agent_rotation(agent_metadata: str,
as_euler: bool = True) -> np.ndarray:
""" Get the agent's rotation.
Args:
agent_metadata (str): Metadata string.
as_euler (bool): True to return zxy euler angles.
Otherwise, return quaternion.
Returns:
np.ndarray: shape (3,) array containing (z, x, y) euler angles.
"""
root = ET.fromstring(agent_metadata)
x = float(root.find("quaternion").attrib["x"])
y = float(root.find("quaternion").attrib["y"])
z = float(root.find("quaternion").attrib["z"])
w = float(root.find("quaternion").attrib["w"])
return Rotation(
(x, y, z, w)).as_euler("zxy") if as_euler else (x, y, z, w)
def read_gyro_fits(gyrohdu):
"""
reads gyro quaternion from fits file hdu and returns AttDATA container
-------
Params:
hdu: fits.BintableHDU containing gyrofiles data
return:
AttDATA container, which bares attitude information
"""
gyrodata = gyrohdu.data
quats = np.array([gyrodata["QORT_%d" % i] for i in [1, 2, 3, 0]]).T
times = gyrodata["TIME"]
masktimes = times > T0
mask0quats = np.sum(quats**2, axis=1) > 0.
mask = np.logical_and(masktimes, mask0quats)
times, quats = times[mask], quats[mask]
ts, uidx = np.unique(times, return_index=True)
return AttDATA(ts, Rotation(quats[uidx]) * qgyro0 * ARTQUATS["GYRO"])
def _handle_pose(self, msg):
if self._tf_buffer.can_transform("bluerov2/base_link", "map_ned",
rospy.Time.now(),
rospy.Duration.from_sec(0.5)):
tf = self._tf_buffer.lookup_transform("bluerov2/base_link",
"map_ned", rospy.Time.now(),
rospy.Duration.from_sec(0.5))
else:
return
# TODO neptus gets confused if you try to give all available estimates, there is probably a transformation problem
# self._estimated_state_msg.x = tf.transform.translation.x
# self._estimated_state_msg.y = tf.transform.translation.y
# self._estimated_state_msg.z = tf.transform.translation.z
# self._estimated_state_msg.depth = tf.transform.translation.z
# self._estimated_state_msg.height = msg.pose.pose.position.z
R = Rotation([
tf.transform.rotation.x, tf.transform.rotation.y,
tf.transform.rotation.z, tf.transform.rotation.w
])
self._estimated_state_msg.phi, self._estimated_state_msg.theta, self._estimated_state_msg.psi = R.as_euler(
"xyz", False).squeeze()
def rotate(self, rotator: np.ndarray):
if isinstance(rotator, list):
rotator = np.array(rotator)
# if 1d array with length 3 -> euler angles
if rotator.shape == (1, 3) or rotator.shape == (3, ):
matrix = Rotation.from_euler('xyz', rotator).as_matrix()
# if 1d array with length 4 -> quaternion
elif rotator.shape == (1, 4) or rotator.shape == (4, ):
matrix = Rotation(rotator).as_matrix()
# if 2d array use transform
elif rotator.shape == (3, 3):
matrix = rotator
else:
raise TypeError(
f'Rotator was passed that is not valid:\n{rotator}')
self.transform(matrix)
return
def __init__(self):
super().__init__('odom_publisher')
# Parameters
self.declare_parameters(namespace='',
parameters=[('axle_length', None),
('wheelbase_length', None),
('wheel_radius', None),
('center_of_mass_offset', 0.0),
('damping_factor', 1)])
try:
self.axle_length = float(self.get_parameter('axle_length').value)
self.wheelbase_length = float(
self.get_parameter('wheelbase_length').value)
self.wheel_radius = float(self.get_parameter('wheel_radius').value)
self.center_of_mass_offset = float(
self.get_parameter('center_of_mass_offset').value)
self.damping_factor = float(
self.get_parameter('damping_factor').value)
except TypeError as e:
raise RuntimeError('Not all parameters are set properly') from e
# Publishers
queue_size = 10
self.publisher = self.create_publisher(Odometry, 'odom', queue_size)
# Subscribers
self.create_subscription(AckermannFeedback, 'feedback',
self.feedback_callback, 10)
self.state = AckermannState(
position=np.array([0, 0, 0]),
orientation=Rotation([0, 0, 0, 1]), # identity
left_wheel_speed=0.0,
right_wheel_speed=0.0,
steering_angle=0.0,
time=self.get_clock().now())
def get_vecs_convex(vecs):
"""
for a set of vectors (which assumed to cover less the pi along any direction)
produces set of vectors which is verteces of convex figure, incapsulating all other vectors on the sphere
------
Params:
vecs - a set of vectos in the form of ... x 3 array
return:
cvec - mean unit vector = SUM vecs / || SUM vecs ||
r1, r2 - two orthogonal vectors, which will define axis dirrection on the sphere surface, at which the convex figure will be searched
quat - quaternion, which puts cvec in the equatorial plane along the shortest trajectory
vecs - a set of vectors, which points to the vertexes of the convex hull on the sphere surface, r, d - quasi cartesian coordinates of the vecteces
it should be noted that convex hull is expected to be alongated along equator after quaternion rotation
"""
cvec = vecs.sum(axis=0)
cvec = cvec / sqrt(np.sum(cvec**2.))
vrot = np.cross(np.array([0., 0., 1.]), cvec)
vrot = vrot / np.sqrt(np.sum(vrot**2.))
#vrot2 = np.cross(vrot, cvec)
#vrot2 = vrot2/sqrt(np.sum(vrot2**2))
alpha = pi / 2. - np.arccos(cvec[2])
quat = Rotation([
vrot[0] * sin(alpha / 2.), vrot[1] * sin(alpha / 2.),
vrot[2] * sin(alpha / 2.),
cos(alpha / 2.)
])
r1 = np.array([0, 0, 1])
r2 = np.cross(quat.apply(cvec), r1)
vecn = quat.apply(vecs) - quat.apply(cvec)
l, b = np.sum(quat.apply(vecs) * r2, axis=1), vecn[:, 2]
ch = ConvexHull(np.array([l, b]).T)
r, d = l[ch.vertices], b[ch.vertices]
return cvec, r1, r2, quat, vecs[ch.vertices], r, d
return quat.quaternion(1, 0, 0, 0)
if __name__ == "__main__":
from scipy.spatial.transform import Rotation
np.random.seed(1234)
fake_gyro_data = np.random.random((1000, 4))
fake_gyro_data[:, 0] = np.arange(1000) / 10
#print(fake_gyro_data)
integrator = GyroIntegrator(fake_gyro_data,
time_scaling=1,
gyro_scaling=4,
zero_out_time=True,
initial_orientation=None,
acc_data=None)
integrator.integrate_all()
stabtransforms = integrator.get_interpolated_stab_transform(0.5)[1]
#print("\n".join([str(q) for q in stabtransforms]))
q = stabtransforms[-1].flatten()
rotmat = np.array([[1, 0, 0], [0, 0, 0], [0, 0, 0]])
rot = Rotation([q[1], q[2], q[3], q[0]]).as_matrix()
final_rotation = np.eye(3)
final_rotation[0, 0] = -1
#combined_rotation[0:3,0:3] = np.linalg.multi_dot([final_rotation, np.linalg.inv(combined_rotation[0:3,0:3]), np.linalg.inv(final_rotation)])
#rot = Rotation([-q[1],-q[2],q[3],-q[0]]).as_matrix()
print(rot)
def get_rotation_map(self, img, quat):
"""Get maps for doing perspective rotations
WORK IN PROGRESS. Currently for testing
"""
# https://stackoverflow.com/a/12293128
# https://en.wikipedia.org/wiki/Homography_(computer_vision)
rotXval = 0
rotYval = 0
rotZval = 0
rotX = (rotXval) * np.pi / 180
rotY = (rotYval) * np.pi / 180
rotZ = (rotZval) * np.pi / 180
rot_mat = np.eye(4)
#print(Rotation([quat[0,1],quat[0,2],quat[0,3],quat[0,0]]).as_euler('xyz'))
quat = quat.flatten()
#eul = Rotation([quat[1],quat[2],quat[3],quat[0]]).as_euler('xyz')
combined_rotation = np.eye(4)
#combined_rotation[0:3,0:3] = Rotation.from_euler('xyz', [eul[0], eul[1], -eul[2]], degrees=False).as_matrix()
combined_rotation[0:3, 0:3] = Rotation(
[-quat[1], -quat[2], quat[3], -quat[0]]).as_matrix()
#eul = Rotation(quat).as_euler('xyz')[0]
#rot1 = np.eye(4)
#rot1[0:3,0:3] = Rotation.from_euler('xyz', [0, -eul[1], 0], degrees=False).as_matrix() #
#rot2 = np.eye(4)
#rot2[0:3,0:3] = Rotation.from_euler('xyz', [eul[2], 0, 0], degrees=False).as_matrix()
#rot3 = np.eye(4)
#rot3[0:3,0:3] = Rotation.from_euler('xyz', [0, 0, eul[0]], degrees=False).as_matrix()
#combined_rotation = np.linalg.multi_dot([rot1, rot2, rot3])
#combined_rotation = Rotation.from_euler('xyz', [-90, -90, -90], degrees=True) * Rotation(quat)
rot_mat = combined_rotation
#rot_mat[0:3,0:3], jac = cv2.Rodrigues(np.array([rotX,rotY,rotZ], dtype=np.float32))
#rot_mat[0,1] = 0
#rot_mat[1,2] = 0
#rot_mat[2,2] = 1
img_dim = img.shape[:2][::-1]
# Scaled 3x4 camera matrix
K = np.zeros((3, 4))
K[:3, :3] = self.K
# should make the rotation match fov change
K[0, 0] = self.new_K[0, 0]
K[1, 1] = self.new_K[1, 1]
#print(K)
K *= img_dim[0] / self.calib_dimension[0]
K[2][2] = 1.0
# compute inverse camera matrix using scaled K
Kinv = np.zeros((4, 3))
Kinv[0:3, 0:3] = inverse_cam_mtx(K[:3, :3])
Kinv[3, :] = [0, 0, 1]
distX = 0
distY = 0
distZ = 0
translation = np.array([[1, 0, 0, distX], [0, 1, 0, distY],
[0, 0, 1, distZ], [0, 0, 0, 1]])
H = np.linalg.multi_dot([K, rot_mat, Kinv])
#trans = rot_mat * translation
#trans[2,2] += self.calib_dimension[1]/2
#transform = self.K * trans
outimg = cv2.warpPerspective(img, H, (img.shape[1], img.shape[0]))
return outimg
return quat.normalize(quat.quaternion(q0,q1,q2,q3))
else:
return quat.quaternion(1,0,0,0)
if __name__ == "__main__":
from scipy.spatial.transform import Rotation
fake_gyro_data = np.random.random((1000,4))
fake_gyro_data[:,0] = np.arange(1000)/10
print(fake_gyro_data)
integrator = GyroIntegrator(fake_gyro_data, time_scaling=1, gyro_scaling=1, zero_out_time=True, initial_orientation=None, acc_data=None)
integrator.integrate_all()
stabtransforms =integrator.get_interpolated_stab_transform(0.5)[1]
orig = stabtransforms[50]
# Hero 6 as reference
fake_gyro_data[:,2] = -fake_gyro_data[:,2]
integrator = GyroIntegrator(fake_gyro_data, time_scaling=1, gyro_scaling=1, zero_out_time=True, initial_orientation=None, acc_data=None)
integrator.integrate_all()
stabtransforms =integrator.get_interpolated_stab_transform(0.5)[1]
weird = stabtransforms[50]
print(weird)
print(orig)
combined_rotation[0:3,0:3] = Rotation([-quart[1],-quart[2],quart[3],-quart[0]]).as_matrix()
from scipy.spatial.transform import Rotation, Slerp
from scipy.spatial import ConvexHull
from scipy.optimize import minimize
from astropy.coordinates import SkyCoord
from astropy.wcs import WCS
from math import cos, sin, pi, sqrt
import numpy as np
from scipy import ndimage
import sys
from multiprocessing import Pool, cpu_count, Queue, Process
MPNUM = cpu_count()
r0 = Rotation([0, 0, 0, 1])
DELTASKY = 15. / 3600. / 180. * pi #previously I set it to be 5''
"""
optica axis is shifted 11' away of the sattelite x axis, therefore we need some more fine resolution
5'' binning at the edge of the detector, is rotation take place around its center is 2*pi/9/24
(hint: pix size 45'', 5''=45''/9)
"""
DELTAROLL = 1. / 24. / 3.
def hist_quat(quat):
"""
provided set of quats split them on several groups, corresponding to a limited sets of optical axis direction on sky and roll angles
the number of groups is defined by two parameters - DELTASKY and DELTAROLL
DELTASKY
def callback(self, data):
translation = data.transform.translation
rotation = data.transform.rotation
translation_vec = [translation.x, translation.y, translation.z]
rotation_vec = [rotation.x, rotation.y, rotation.z, rotation.w]
R = {"tran": translation_vec, "quart": rotation_vec}
self.queue.append(R)
if len(self.queue) < 10:
pass
else:
mean_translation_x = 0.0
mean_translation_y = 0.0
mean_translation_z = 0.0
mean_rotation_x = 0.0
mean_rotation_y = 0.0
mean_rotation_z = 0.0
mean_rotation_w = 0.0
for t in self.queue:
mean_translation_x += t['tran'][0]
mean_translation_y += t['tran'][1]
mean_translation_z += t['tran'][2]
mean_rotation_x += t['quart'][0]
mean_rotation_y += t['quart'][1]
mean_rotation_z += t['quart'][2]
mean_rotation_w += t['quart'][3]
mean_translation_x /= 10
mean_translation_y /= 10
mean_translation_z /= 10
mean_rotation_x /= 10
mean_rotation_y /= 10
mean_rotation_z /= 10
mean_rotation_w /= 10
mean_translation_x = -1.0 * mean_translation_x
# 四元数转旋转矩阵
rotation_matrix = Rotation([
mean_rotation_x, mean_rotation_y, mean_rotation_z,
mean_rotation_w
]).as_matrix()
T = np.identity(4, dtype=np.float32)
# T[:3 , :3] = rotation_matrix
T[0, 0] = rotation_matrix[0, 0]
T[0, 1] = rotation_matrix[0, 1]
T[0, 2] = rotation_matrix[0, 2]
T[1, 0] = rotation_matrix[1, 0]
T[1, 1] = rotation_matrix[1, 1]
T[1, 2] = rotation_matrix[1, 2]
T[2, 0] = rotation_matrix[2, 0]
T[2, 1] = rotation_matrix[2, 1]
T[2, 2] = rotation_matrix[2, 2]
T[0, 3] = mean_translation_x
T[1, 3] = mean_translation_y
T[2, 3] = mean_translation_z
T = np.mat(T).I
print("T: ", T)
print("*********************************")
print("object 2 base: ")
print(object_2_base_link)
print('---------------------------------')
# print("camera to base-link trans: ", T @ object_2_base_link)
print("camera to base-link trans: ", base_link_2_object_matrix @ T)
self.queue = []
def __call__(self, tarr):
return Rotation(np.empty((0, 4), np.double)) if np.asarray(
tarr).size == 0 else super().__call__(tarr)
ARTCALDBPATH = os.environ["ARTCALDB"]
indexfname = "artxc_index.fits"
TELTOURD = {v:k for k, v in URDTOTEL.items()}
idxdata = fits.getdata(os.path.join(ARTCALDBPATH, indexfname), 1)
idxtabl = Table(idxdata).to_pandas()
idxtabl["CAL_DATE"] = pandas.to_datetime(idxtabl["CAL_DATE"])
idxtabl.set_index("CAL_DATE", inplace=True)
CUTAPP = None
FLATVIGN = False
FLATBKG = False
qbokz0 = Rotation([0., -0.707106781186548, 0., 0.707106781186548])
qgyro0 = Rotation([0., 0., 0., 1.])
OPAX = np.array([1, 0, 0])
ARTQUATS = {row[0]:Rotation(row[1:]) for row in fits.getdata(os.path.join("/srg/a1/work/andrey/ART-XC/Quats_V5", "ART_QUATS_V5_rotin.fits"), 1)}
ARTQUATS.update({TELTOURD[row[0]]:Rotation(row[1:]) for row in fits.getdata(os.path.join("/srg/a1/work/andrey/ART-XC/Quats_V5", "ART_QUATS_V5_rotin.fits"), 1) if row[0] in TELTOURD})
"""
some magical numbers, generally define mean count rate of the background of each detector relative to the mean over all seven
"""
urdbkgsc = {28: 1.0269982359153347,
22: 0.9461951470620872,
23: 1.0291298607731770,
24: 1.0385034889253482,
25: 0.9769294100898714,
26: 1.0047417556512688,
def main(calib_path_1, calib_path_2):
with open(calib_path_1, 'r') as c_1, open(calib_path_2, 'r') as c_2:
calib0 = json.load(c_1)
calib1 = json.load(c_2)
for i, (t_imu_cam_0, t_imu_cam_1) in enumerate(
zip(calib0['value0']['T_imu_cam'], calib1['value0']['T_imu_cam'])):
print(f'\nCamera {i} transformation differences')
t_0 = np.array(list(t_imu_cam_0.values())[0:2])
t_1 = np.array(list(t_imu_cam_1.values())[0:2])
r_0 = Rotation(list(t_imu_cam_0.values())[3:7])
r_1 = Rotation(list(t_imu_cam_1.values())[3:7])
print_abs_rel(f'Transformation', t_0, t_1)
print_abs_rel(f'Rotation', r_0.as_rotvec(), r_1.as_rotvec())
for i, (intrinsics0, intrinsics1) in enumerate(
zip(calib0['value0']['intrinsics'], calib1['value0']['intrinsics'])):
print(f'\nCamera {i} intrinsics differences')
for (
k_0, v_0), (_, v_1) in zip(
intrinsics0['intrinsics'].items(), intrinsics1['intrinsics'].items()):
print_abs_rel(f'Difference for {k_0}', v_0, v_1)
print_abs_rel('\nAccel Bias Difference',
np.array(calib0['value0']['calib_accel_bias'][0:2]),
np.array(calib1['value0']['calib_accel_bias'][0:2]))
print_abs_rel('Accel Scale Difference',
np.array(calib0['value0']['calib_accel_bias'][3:9]),
np.array(calib1['value0']['calib_accel_bias'][3:9]))
print_abs_rel('Gyro Bias Difference',
np.array(calib0['value0']['calib_gyro_bias'][0:2]),
np.array(calib1['value0']['calib_gyro_bias'][0:2]))
print_abs_rel('Gyro Scale Difference',
np.array(calib0['value0']['calib_gyro_bias'][3:12]),
np.array(calib1['value0']['calib_gyro_bias'][3:12]))
print_abs_rel(
'\nAccel Noise Std Difference',
calib0['value0']['accel_noise_std'],
calib1['value0']['accel_noise_std'])
print_abs_rel(
'Gyro Noise Std Difference',
calib0['value0']['gyro_noise_std'],
calib1['value0']['gyro_noise_std'])
print_abs_rel(
'Accel Bias Std Difference',
calib0['value0']['accel_bias_std'],
calib1['value0']['accel_bias_std'])
print_abs_rel(
'Gyro Bias Std Difference',
calib0['value0']['gyro_bias_std'],
calib1['value0']['gyro_bias_std'])
print_abs_rel(
'\nCam Time Offset Difference',
calib0['value0']['cam_time_offset_ns'],
calib0['value0']['cam_time_offset_ns'])
