def get_ar_marker_poses (rgb, depth, pc=None):
"""
In order to run this, ar_marker_service needs to be running.
"""
global getMarkers, req, ar_initialized
if not ar_initialized:
if rospy.get_name() == '/unnamed':
rospy.init_node('ar_tfm')
getMarkers = rospy.ServiceProxy("getMarkers", MarkerPositions)
req = MarkerPositionsRequest()
ar_initialized = True
if pc is None:
xyz = clouds.depth_to_xyz(depth, asus_xtion_pro_f)
pc = ru.xyzrgb2pc(xyz, rgb, '/camera_frame')
req.pc = pc
marker_tfm = {}
res = getMarkers(req)
for marker in res.markers.markers:
marker_tfm[marker.id] = conversions.pose_to_hmat(marker.pose.pose)
return marker_tfm
def do_icp(surface_points):
#Random point set, scaled up
p1 = surface_points
n = p1.shape[0]
# Transform the points by some random transform
tfm = make_transform(trans_SCALE=1)
p2 = np.c_[p1,np.ones((n,1))].dot(tfm.T)[:,0:3]
#print p1
#print p2
print tfm
noise = make_transform(0.01,0.1)
req = ICPTransformRequest()
req.pc1 = ru.xyz2pc(p1, 'a')
req.pc2 = ru.xyz2pc(p2, 'b')
req.guess = conversions.hmat_to_pose(tfm.dot(noise))
res = findTransform(req)
ntfm = conversions.pose_to_hmat(res.pose)
print ntfm
print ntfm.dot(np.linalg.inv(tfm))
print np.linalg.norm(ntfm.dot(np.linalg.inv(tfm)) - np.eye(4))
return np.linalg.norm(np.abs(ntfm[0:3,3]-tfm[0:3,3]))
def get_ar_transform_id (depth, rgb, idm=None):
"""
In order to run this, ar_marker_service needs to be running.
"""
req.pc = ru.xyzrgb2pc(clouds.depth_to_xyz(depth, asus_xtion_pro_f), rgb, '/camera_link')
res = getMarkers(req)
marker_tfm = {marker.id:conversions.pose_to_hmat(marker.pose.pose) for marker in res.markers.markers}
if not idm: return marker_tfm
if idm not in marker_tfm: return None
return marker_tfm[idm]
def get_marker_transforms(self, markers=None, time_thresh=1.0, get_time=False):
"""
Get the transforms for markers denoted by their ids (markers)
Threshold represents the tolerance for stale transforms.
"""
time_now = rospy.Time.now().to_sec()
if self.latest_markers is None or time_now - self.latest_time > time_thresh:
if get_time:
return {}, self.latest_time
else:
return {}
if markers is None:
marker_transforms = {marker.id:conversions.pose_to_hmat(marker.pose.pose)\
for marker in self.latest_markers.markers if marker.id != 0}
else:
marker_transforms = {marker.id:conversions.pose_to_hmat(marker.pose.pose)\
for marker in self.latest_markers.markers if marker.id in markers and marker.id != 0}
if not get_time:
return marker_transforms
else:
return marker_transforms, self.latest_time
def get_ar_marker_poses (pc):
global getMarkers, req
if rospy.get_name() == '/unnamed':
rospy.init_node('keypoints')
if getMarkers is None:
getMarkers = rospy.ServiceProxy("getMarkers", MarkerPositions)
req.pc = pc
marker_tfm = {}
res = getMarkers(req)
for marker in res.markers.markers:
marker_tfm[marker.id] = conversions.pose_to_hmat(marker.pose.pose).tolist()
#print "Marker ids found: ", marker_tfm.keys()
return marker_tfm
def get_ar_marker_poses (msg, ar_markers = None, use_pc_service=True, model="", track=False):
'''
get poses according to ar_markers
if ar_markers == None, then for all ar markers appeared in the point cloud
'''
global getMarkersPC, getImageMarkers, reqPC, reqImage, bridge
if model is None:
return "No camera model provided for extraction."
if rospy.get_name() == '/unnamed':
rospy.init_node('ar_marker_poses', anonymous=True)
if use_pc_service:
if getMarkersPC is None:
getMarkersPC = rospy.ServiceProxy("getMarkers", MarkerPositions)
reqPC.pc = msg
reqPC.track = track
res = getMarkersPC(reqPC)
else:
if model not in getImageMarkers:
getImageMarkers[model] = rospy.ServiceProxy("getImageMarkers"+model, MarkerImagePositions)
if bridge is None:
bridge = CvBridge()
img = bridge.cv_to_imgmsg(msg)
reqImage.img = img
reqImage.track = track
try:
res = getImageMarkers[model](reqImage)
except Exception as e:
redprint("Something went wrong with image" )
print e
return {}
marker_tfm = {}
for marker in res.markers.markers:
if ar_markers == None or marker.id in ar_markers:
marker_tfm[marker.id] = conversions.pose_to_hmat(marker.pose.pose).tolist()
return marker_tfm
def get_ar_transform_id_unorganized (xyz, rgb, idm=None):
"""
In order to run this, ar_marker_service needs to be running.
"""
print xyz
print rgb
xyz = xyz.reshape(xyz.shape[0]*xyz.shape[1], xyz.shape[2])
rgb = rgb.reshape(rgb.shape[0]*rgb.shape[1], rgb.shape[2])
print xyz
print rgb
req.pc = ru.xyzrgb2pc(xyz, rgb, '/camera_link')
res = getMarkers(req)
marker_tfm = {marker.id:conversions.pose_to_hmat(marker.pose.pose) for marker in res.markers.markers}
if not idm: return marker_tfm
if idm not in marker_tfm: return None
return marker_tfm[idm]
def get_ar_marker_poses (rgb, depth):
"""
In order to run this, ar_marker_service needs to be running.
"""
if rospy.get_name() == '/unnamed':
rospy.init_node('keypoints')
getMarkers = rospy.ServiceProxy("getMarkers", MarkerPositions)
#xyz = svi.transform_pointclouds(depth, tfm)
xyz = clouds.depth_to_xyz(depth, asus_xtion_pro_f)
pc = ru.xyzrgb2pc(xyz, rgb, '/base_footprint')
req = MarkerPositionsRequest()
req.pc = pc
marker_tfm = {}
res = getMarkers(req)
for marker in res.markers.markers:
marker_tfm[marker.id] = conversions.pose_to_hmat(marker.pose.pose)
#print "Marker ids found: ", marker_tfm.keys()
return marker_tfm
from hd_utils import ros_utils as ru, conversions
np.set_printoptions(precision=5, suppress=True)
rospy.init_node('test_pcd')
xyzrgb1 = cyni.readPCD('/home/sibi/sandbox/pcd_service/clouds/cloud1.pcd')
pc1 = ru.xyzrgb2pc(xyzrgb1[:,:,0:3],xyzrgb1[:,:,3:6],'')
xyzrgb2 = cyni.readPCD('/home/sibi/sandbox/pcd_service/clouds/cloud2.pcd')
pc2 = ru.xyzrgb2pc(xyzrgb2[:,:,0:3],xyzrgb2[:,:,3:6],'')
mserv = rospy.ServiceProxy('getMarkers', MarkerPositions)
req = MarkerPositionsRequest()
req.pc = pc1
res1 = mserv(req)
req.pc = pc2
res2 = mserv(req)
m1 = res1.markers.markers[1]
m2 = res2.markers.markers[1]
tfm1 = conversions.pose_to_hmat(m1.pose.pose)
tfm2 = conversions.pose_to_hmat(m2.pose.pose)
bb = np.array([[4.807, -1.239, -4.11,3.495],
[3.869,0.2728,-3.501,3.155],
[5.462,-0.8972,-6.1051,4.372],
[0,0,0,1]])
