def __init__(self, R=np.eye(3), t=np.zeros(3)):
"""
Pose is defined as p_cw (pose of the world wrt camera)
"""
p = RigidTransform.from_Rt(R, t)
RigidTransform.__init__(self, xyzw=p.quat.to_xyzw(), tvec=p.tvec)
self.__cached_inverse = None
def tsukuba_load_poses(fn):
"""
Retrieve poses
X Y Z R P Y - > X -Y -Z R -P -Y
np.deg2rad(p[3]),-np.deg2rad(p[4]),-np.deg2rad(p[5]),
p[0]*.01,-p[1]*.01,-p[2]*.01, axes='sxyz') for p in P ]
"""
P = np.loadtxt(os.path.expanduser(fn), dtype=np.float64, delimiter=',')
return [ RigidTransform.from_rpyxyz(np.pi, 0, 0, 0, 0, 0) * \
RigidTransform.from_rpyxyz(
np.deg2rad(p[3]),np.deg2rad(p[4]),np.deg2rad(p[5]),
p[0]*.01,p[1]*.01,p[2]*.01, axes='sxyz') * \
RigidTransform.from_rpyxyz(np.pi, 0, 0, 0, 0, 0) for p in P ]
def __init__(self):
self._channel_uid = dict()
self._sensor_pose = dict()
self.reset_visualization()
self.publish_sensor_frame('camera',
pose=VisualizationMsgsPub.CAMERA_POSE)
self.publish_sensor_frame('origin', pose=RigidTransform.identity())
def set_pose(self, left_pose):
"""
Provide extrinsics to the left camera
"""
super(StereoCamera, self).set_pose(left_pose)
self.right.set_pose(
self.left.oplus(
RigidTransform.from_rpyxyz(0, 0, 0, self.baseline, 0, 0)))
def from_left_with_baseline(cls, lcamera, baseline):
""" Left extrinsics is assumed to be identity """
rcamera = Camera.from_intrinsics_extrinsics(
lcamera.intrinsics,
CameraExtrinsic.from_rigid_transform(
lcamera.oplus(
RigidTransform.from_rpyxyz(0, 0, 0, baseline, 0, 0))))
return cls(lcamera, rcamera, baseline=baseline)
def load_poses(fn):
"""
poses: image_id tx ty tz qx qy qz qw
"""
X = (np.fromfile(fn, dtype=np.float64, sep=' ')).reshape(-1, 8)
abs_poses = map(lambda p: RigidTransform(xyzw=p[4:], tvec=p[1:4]),
X)
rel_poses = [abs_poses[0].inverse() * p for p in abs_poses]
return rel_poses
def predict(self, im, return_rigid_transform=True):
self.forward(im)
predicted_q = self.extract(layer='cls3_fc_wpqr')
predicted_x = self.extract(layer='cls3_fc_xyz')
predicted_q_norm = predicted_q / np.linalg.norm(predicted_q)
if return_rigid_transform:
return RigidTransform(Quaternion.from_wxyz(predicted_q_norm), tvec=predicted_x)
return (predicted_q_norm, predicted_x)
def read_pose(gt):
cam_dir, cam_up = gt.cam_dir, gt.cam_up
z = cam_dir / np.linalg.norm(cam_dir)
x = np.cross(cam_up, z)
y = np.cross(z, x)
R = np.vstack([x, y, z]).T
t = gt.cam_pos / 1000.0
return RigidTransform.from_Rt(R, t)
def drawParticles2d(self,
V,
frame_id='origin',
name='particles',
lname='yaw',
c=['b'],
lc=['r'],
ll=0.05):
LD = []
if len(V) < 1:
print 'drawParticles2d -- no data? skip'
return
d, N = V[0].shape
l = len(V)
for v in V:
Dv = np.vstack((v[0:2, :], np.zeros((1, N)))).T
LD.append(Dv)
publish_cloud(name, LD, c=c, frame_id=frame_id)
if d > 2:
count = -1
DV = np.zeros((d, l * N))
DT = np.zeros((d, l * N))
for v in V:
tt = np.zeros((d, N))
for i in range(0, N):
t = Pose.from_rigid_transform(
0,
RigidTransform(xyzw=Quaternion.from_rpy(
0, 0, v[2, i]).to_xyzw(),
tvec=[v[0, i], v[1, i], 0]))
pert = Pose.from_rigid_transform(
0, RigidTransform(tvec=[ll, 0, 0]))
tv = (t.oplus(pert)).tvec
tt[0:2, i] = tv[0:2]
count += 1
DT[0:d, (count * N):((count + 1) * N)] = tt
Dv = np.vstack((v[0:2, :], np.zeros((1, N))))
DV[0:d, (count * N):((count + 1) * N)] = Dv
publish_line_segments(lname,
DV.T,
DT.T,
c=lc[0],
frame_id=frame_id)
def load_poses(fn, version):
""" Retrieve poses for each scene """
if version == 'v1':
P = np.loadtxt(os.path.expanduser(fn),
usecols=(2, 3, 4, 5, 6, 7, 8),
dtype=np.float64)
return map(
lambda p: RigidTransform(Quaternion.from_wxyz(p[:4]), p[4:]
), P)
elif version == 'v2':
P = np.loadtxt(os.path.expanduser(fn), dtype=np.float64)
return map(
lambda p: RigidTransform(Quaternion.from_wxyz(p[:4]), p[4:]
), P)
else:
raise ValueError(
'''Version %s not supported. '''
'''Check dataset and choose either v1 or v2 scene dataset'''
% version)
def decode(self, msg):
if self.__gazebo_index is None:
for j, name in enumerate(msg.name):
if name == 'mobile_base':
self.__gazebo_index = j
break
pose = msg.pose[self.__gazebo_index]
tvec, ori = pose.position, pose.orientation
return RigidTransform(xyzw=[ori.x, ori.y, ori.z, ori.w],
tvec=[tvec.x, tvec.y, tvec.z])
def on_tags_detection_cb(self, tag_array, tf_dict=None):
if self.gotkf == 0:
return
elif self.gotkf == 1:
self.gotkf -= 1
else:
ValueError("Tags and keyframes out of sync")
#print "DETECTING TAGS"
detections = tag_array.detections
if len(detections) == 0:
return # don't do anything
tag_ids = [d.id for d in detections]
frame_ct = tag_array.detections[0].pose.header.frame_id # frame of the tag in the camera ref
rospy.logerr('Tag detections of {}'.format(tag_ids))
poses_ct =[RigidTransform(xyzw=[d.pose.pose.orientation.x,
d.pose.pose.orientation.y,
d.pose.pose.orientation.z,
d.pose.pose.orientation.w],
tvec=[d.pose.pose.position.x,
d.pose.pose.position.y,
d.pose.pose.position.z]) for d in detections]
if not tf_dict:
if (self.transformer.frameExists('/base_link') and self.transformer.frameExists(frame_ct)):
t = self.transformer.getLatestCommonTime(frame_ct,'/base_link')
try:
(trans, rot) = self.transformer.lookupTransform('/base_link', frame_ct, t)
pose_bc = RigidTransform(xyzw=rot, tvec=trans) # pose of the camera in the body ref frame
except:
print sys.exc_info()
rospy.logerr('could not get camera_rgb to base_link transform')
return
else:
e = tf_dict[0]#tf_dict[from, to] = (trans, rot) in the form of a RigidTransform
pose_bc = RigidTransform(xyzw=e.quat, tvec=e.tvec).inverse() #pose of camera in body frame
poses = [pose_bc*pose_ct for pose_ct in poses_ct] # rigidtransforms for tag_ids, in body frame
for i in range(len(tag_ids)):
x = poses[i].t[0]
y = poses[i].t[1]
self.neo4j_iface.add_measurement(bearingRangeFactorPose2Point2(tag_ids[i], math.atan2(y,x), math.sqrt(x*x + y*y), bearingRangeNoise))
def on_odom_cb(self, data):
r = RigidTransform(xyzw=[data.pose.pose.orientation.x,
data.pose.pose.orientation.y,
data.pose.pose.orientation.z,
data.pose.pose.orientation.w],
tvec=[data.pose.pose.position.x,
data.pose.pose.position.y,
data.pose.pose.position.z]) # current odom
if not self.old_odom:
self.old_odom = r
# no odom diff set
else:
self.odom_diff = self.old_odom.inverse()*r
def get_noise():
if len(noise) == 6:
xyz = np.random.normal(0, noise[:3])
rpy = np.random.normal(0, noise[3:])
elif len(noise) == 2:
xyz = np.random.normal(0, noise[0], size=3) \
if noise[0] > 0 else np.zeros(3)
rpy = np.random.normal(0, noise[1], size=3) \
if noise[1] > 0 else np.zeros(3)
else:
raise ValueError('Unknown noise length, either 2, or 6')
return RigidTransform.from_rpyxyz(rpy[0], rpy[1], rpy[2], xyz[0],
xyz[1], xyz[2])
def visualize_ground_truth(self):
import pybot.externals.lcm.draw_utils as draw_utils
# Publish ground truth poses, and aligned point clouds
draw_utils.publish_pose_list('ground_truth_poses', self.poses)
# carr = get_color_by_label(scene.map_info.labels)
draw_utils.publish_cloud(
'aligned_cloud', [obj.points for obj in self.map_info.objects],
c=[obj.colors for obj in self.map_info.objects])
draw_utils.publish_pose_list(
'aligned_poses', [
RigidTransform(tvec=obj.center)
for obj in self.map_info.objects
],
texts=[str(obj.label) for obj in self.map_info.objects])
def draw_laser_frustum(pose, zmin=0.0, zmax=10, fov=np.deg2rad(60)):
N = 30
curve = np.vstack([(RigidTransform.from_rpyxyz(0, 0, rad, 0, 0, 0) *
np.array([[zmax, 0, 0]]))
for rad in np.linspace(-fov / 2, fov / 2, N)])
curve_w = pose * curve
faces, edges = [], []
for cpt1, cpt2 in zip(curve_w[:-1], curve_w[1:]):
faces.extend([pose.translation, cpt1, cpt2])
edges.extend([cpt1, cpt2])
# Connect the last pt in the curve w/ the current pose,
# then connect the the first pt in the curve w/ the curr. pose
edges.extend([edges[-1], pose.translation])
edges.extend([edges[0], pose.translation])
faces = np.vstack(faces)
edges = np.vstack(edges)
return (faces, edges)
def oxts_process_cb(self, fn):
X = np.fromfile(fn, dtype=np.float64, sep=' ')
packet = AttrDict({fmt: x
for (fmt, x) in zip(self.oxts_formats, X)})
# compute scale from first lat value
if self.scale_ is None:
self.scale_ = np.cos(packet.lat * np.pi / 180.)
# Use a Mercator projection to get the translation vector
tx = self.scale_ * packet.lon * np.pi * EARTH_RADIUS / 180.
ty = self.scale_ * EARTH_RADIUS * \
np.log(np.tan((90. + packet.lat) * np.pi / 360.))
tz = packet.alt
t = np.array([tx, ty, tz])
# We want the initial position to be the origin, but keep the ENU
# coordinate system
rx, ry, rz = packet.roll, packet.pitch, packet.yaw
Rx = np.float32([1,0,0, 0,
np.cos(rx), -np.sin(rx), 0,
np.sin(rx), np.cos(rx)]).reshape(3,3)
Ry = np.float32([np.cos(ry),0,np.sin(ry),
0, 1, 0,
-np.sin(ry), 0, np.cos(ry)]).reshape(3,3)
Rz = np.float32([np.cos(rz), -np.sin(rz), 0,
np.sin(rz), np.cos(rz), 0,
0, 0, 1]).reshape(3,3)
R = np.dot(Rz, Ry.dot(Rx))
pose = RigidTransform.from_Rt(R, t)
if self.p_init_ is None:
self.p_init_ = pose.inverse()
# Use the Euler angles to get the rotation matrix
return AttrDict(packet=packet, pose=self.p_init_ * pose)
def kitti_load_poses(fn):
X = (np.fromfile(fn, dtype=np.float64, sep=' ')).reshape(-1,12)
return [RigidTransform.from_Rt(p[:3,:3], p[:3,3])
for p in [x.reshape(3,4) for x in X]]
def on_tags_detection_cb(self, tag_array, tf_dict=None):
#print "DETECTING TAGS"
detections = tag_array.detections
if len(detections) == 0:
return # don't do anything
tag_ids = [d.id for d in detections]
frame_ct = tag_array.detections[
0].pose.header.frame_id # frame of the tag in the camera ref
rospy.logerr('Tag detections of {}'.format(tag_ids))
poses_ct = [
RigidTransform(xyzw=[
d.pose.pose.orientation.x, d.pose.pose.orientation.y,
d.pose.pose.orientation.z, d.pose.pose.orientation.w
],
tvec=[
d.pose.pose.position.x, d.pose.pose.position.y,
d.pose.pose.position.z
]) for d in detections
]
if not tf_dict:
if (self.transformer.frameExists('/base_link')
and self.transformer.frameExists(frame_ct)):
t = self.transformer.getLatestCommonTime(
frame_ct, '/base_link')
try:
(trans, rot) = self.transformer.lookupTransform(
'/base_link', frame_ct, t)
pose_bc = RigidTransform(
xyzw=rot,
tvec=trans) # pose of the camera in the body ref frame
except:
print sys.exc_info()
rospy.logerr(
'could not get camera_rgb to base_link transform')
return
else:
e = tf_dict[
0] #tf_dict[from, to] = (trans, rot) in the form of a RigidTransform
pose_bc = RigidTransform(
xyzw=e.quat,
tvec=e.tvec).inverse() #pose of camera in body frame
poses = [pose_bc * pose_ct for pose_ct in poses_ct
] # rigidtransforms for tag_ids, in body frame
for i in range(len(tag_ids)):
x = poses[i].t[0]
y = poses[i].t[1]
#rospy.logerr('\n\n tag is {}'.format(tag_ids[i]))
self.session.run(
"MERGE (l1:NEWDATA:LAND:SESSTURTLE {frtend: {land_info}}) "
"MERGE (o1:POSE:NEWDATA:SESSTURTLE {frtend: {var_info}}) "
"MERGE (f:FACTOR:NEWDATA:SESSTURTLE {frtend: {fac_info}}) " # odom-landmark factor
"MERGE (o1)-[:DEPENDENCE]->(f) "
"MERGE (f)-[:DEPENDENCE]->(l1) "
"RETURN id(o1)",
{
"land_info":
json.dumps({
"t": "L",
"tag_id": tag_ids[i],
"userready": 0
}),
"var_info":
json.dumps({
"t": "P",
"uid": self.idx_,
"userready": 0
}),
"fac_info":
json.dumps({
"t":
"F",
"lklh":
"BR G 2",
"meas":
str(math.atan2(y, x)) + " " +
str(math.sqrt(x * x + y * y)) + " 1e-3 0 1e-2",
"userready":
0,
"btwn":
str(self.idx_) + " " + str(tag_ids[i])
})
})
def odom_decode(data):
tvec, ori = data.pose.position, data.pose.orientation
return RigidTransform(xyzw=[ori.x, ori.y, ori.z, ori.w],
tvec=[tvec.x, tvec.y, tvec.z])
def __repr__(self):
return 'CameraExtrinsic =======>\npose = {:}'.format(
RigidTransform.__repr__(self))
def establish_tfs(self, relations):
"""
Perform a one-time look up of all the requested
*static* relations between frames (available via /tf)
"""
# Init node and tf listener
rospy.init_node(self.__class__.__name__, disable_signals=True)
tf_listener = tf.TransformListener()
# Create tf decoder
tf_dec = TfDecoderAndPublisher(channel='/tf')
# Establish tf relations
print('{} :: Establishing tfs from ROSBag'.format(
self.__class__.__name__))
for self.idx, (channel, msg,
t) in enumerate(self.log.read_messages(topics='/tf')):
tf_dec.decode(msg)
for (from_tf, to_tf) in relations:
# If the relations have been added already, skip
if (from_tf, to_tf) in self.relations_map_:
continue
# Check if the frames exist yet
if not tf_listener.frameExists(
from_tf) or not tf_listener.frameExists(to_tf):
continue
# Retrieve the transform with common time
try:
tcommon = tf_listener.getLatestCommonTime(from_tf, to_tf)
(trans, rot) = tf_listener.lookupTransform(
from_tf, to_tf, tcommon)
self.relations_map_[(from_tf,
to_tf)] = RigidTransform(tvec=trans,
xyzw=rot)
# print('\tSuccessfully received transform: {:} => {:} {:}'
# .format(from_tf, to_tf, self.relations_map_[(from_tf,to_tf)]))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
# print e
pass
# Finish up once we've established all the requested tfs
if len(self.relations_map_) == len(relations):
break
try:
tfs = [
self.relations_map_[(from_tf, to_tf)]
for (from_tf, to_tf) in relations
]
for (from_tf, to_tf) in relations:
print(
'\tSuccessfully received transform:\n\t\t {:} => {:} {:}'.
format(from_tf, to_tf,
self.relations_map_[(from_tf, to_tf)]))
except:
raise RuntimeError('Error concerning tf lookup')
print('{} :: Established {:} relations\n'.format(
self.__class__.__name__, len(tfs)))
return tfs
class VisualizationMsgsPub:
"""
Visualization publisher class
"""
CAMERA_POSE = RigidTransform.from_rpyxyz(-np.pi / 2,
0,
-np.pi / 2,
0,
0,
2,
axes='sxyz')
XZ_POSE = RigidTransform.from_rpyxyz(np.pi / 2, 0, 0, 0, 0, 0, axes='sxyz')
def __init__(self):
self._channel_uid = dict()
self._sensor_pose = dict()
self.reset_visualization()
self.publish_sensor_frame('camera',
pose=VisualizationMsgsPub.CAMERA_POSE)
self.publish_sensor_frame('origin', pose=RigidTransform.identity())
# self.publish_sensor_frame('origin_xz', pose=VisualizationMsgsPub.XZ_POSE)
def publish(self, channel, data):
publish(channel, data)
def list_frames(self):
return self._sensor_pose
def channel_uid(self, channel):
uid = self._channel_uid.setdefault(channel, len(self._channel_uid))
return uid + 1000
# def sensor_uid(self, channel):
# uid = self._sensor_uid.setdefault(channel, len(self._sensor_uid))
# return uid
def has_sensor_pose(self, channel):
return channel in self._sensor_pose
def get_sensor_pose(self, channel):
return self._sensor_pose[channel].copy()
def set_sensor_pose(self, channel, pose):
self._sensor_pose[channel] = pose
def reset_visualization(self):
print('{} :: Reseting Visualizations'.format(self.__class__.__name__))
msg = vs.reset_collections_t()
self.publish("RESET_COLLECTIONS", serialize(msg))
def publish_sensor_frame(self, channel, pose=None):
"""
Publish sensor frame in which the point clouds
are drawn with reference to. sensor_frame_msg.id is hashed
by its channel (may be collisions since its right shifted by 32)
"""
# Sensor frames msg
msg = vs.obj_collection_t()
msg.id = self.channel_uid(channel)
msg.name = 'BOTFRAME_' + channel
msg.type = vs.obj_collection_t.AXIS3D
msg.reset = True
# Send sensor pose
pose_msg = vs.obj_t()
roll, pitch, yaw, x, y, z = pose.to_rpyxyz(axes='sxyz')
pose_msg.id = 0
pose_msg.x, pose_msg.y, pose_msg.z, \
pose_msg.roll, pose_msg.pitch, pose_msg.yaw = x, y, z, roll, pitch, yaw
# Save pose
self.set_sensor_pose(channel, pose)
# msg.objs = [pose_msg]
msg.objs.extend([pose_msg])
msg.nobjs = len(msg.objs)
self.publish("OBJ_COLLECTION", serialize(msg))
def rt_from_geom_pose(msg):
xyzw = msg.orientation
xyz = msg.position
return RigidTransform(tvec=np.array([xyz.x, xyz.y, xyz.z]),
xyzw=[xyzw.x, xyzw.y, xyzw.z, xyzw.w])
import time
import numpy as np
from pybot.geometry.rigid_transform import RigidTransform, Pose
import pybot.externals.draw_utils as draw_utils
if __name__ == "__main__":
print('Reset')
time.sleep(1)
print('Create new sensor frame')
draw_utils.publish_sensor_frame('new_frame', RigidTransform(tvec=[0,0,1]))
poses = [RigidTransform.from_rpyxyz(np.pi/180 * 10*j, 0, 0, j * 1.0, 0, 0)
for j in range(10)]
draw_utils.publish_pose_list('poses', poses, frame_id='origin')
draw_utils.publish_pose_list('poses_new_frame', poses, frame_id='new_frame')
print('Create point cloud with colors')
X = np.random.rand(1000, 3)
C = np.hstack((np.random.rand(1000, 1), np.zeros((1000,2))))
draw_utils.publish_cloud('cloud', X, c='b',
frame_id='new_frame', reset=True)
draw_utils.publish_line_segments('lines', X[:-1], X[1:],
c='y', frame_id='new_frame')
time.sleep(2)
print('Overwriting point cloud')
X = np.random.rand(1000, 3) + np.float32([5, 0, 0])
C = np.hstack((np.random.rand(1000, 1), np.zeros((1000,2))))
def kitti_load_poses(fn):
X = (np.fromfile(fn, dtype=np.float64, sep=' ')).reshape(-1,12)
return map(lambda p: RigidTransform.from_Rt(p[:3,:3], p[:3,3]),
map(lambda x: x.reshape(3,4), X))
