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]])