def __init__(self, args):
rospy.init_node('picmap_server')
stereo_cam = camera.Camera(
(389.0, 389.0, 89.23 * 1e-3, 323.42, 323.42, 274.95))
self.vo = None
self.transformer = pytf_swig.pyTransformer()
self.transformer.setExtrapolationLimit(0.0)
self.fd = FeatureDetectorFast(300)
self.ds = DescriptorSchemeCalonder()
self.pm = PictureMap(self.ds)
#self.pub = rospy.Publisher("/picmap_pose", vslam.msg.Picmap)
rospy.Subscriber('/stereo/raw_stereo',
stereo_msgs.msg.RawStereo,
self.handle_raw_stereo_queue,
queue_size=2,
buff_size=7000000)
rospy.Subscriber('/amcl_pose',
geometry_msgs.msg.PoseWithCovarianceStamped,
self.handle_amcl_pose)
rospy.Subscriber('/tf_message', tf.msg.tfMessage, self.handle_tf_queue)
self.pmlock = threading.Lock()
self.q = Queue.Queue(0) # 0 means maxsize is infinite
self.tfq = Queue.Queue(0)
t = threading.Thread(target=self.worker)
t.start()
def __init__(self):
rospy.Subscriber('/stereo/raw_stereo', stereo_msgs.msg.RawStereo, self.handle_raw_stereo, queue_size=2, buff_size=7000000)
self.pub_vo = rospy.Publisher("/vo", deprecated_msgs.msg.VOPose)
self.vo = None
self.modulo = 0
self.fd = FeatureDetectorFast(300)
self.ds = DescriptorSchemeCalonder()
def params(self, pmsg):
if not self.vo:
self.cam = camera.VidereCamera(pmsg.data)
if DESCRIPTOR == 'CALONDER':
self.vo = VisualOdometer(
self.cam, descriptor_scheme=DescriptorSchemeCalonder())
self.desc_diff_thresh = 0.001
elif DESCRIPTOR == 'SAD':
self.vo = Tracker(self.cam)
self.desc_diff_thresh = 2000.0
else:
print "Unknown descriptor"
return
import math
import random
import time
random.seed(0)
pg = TreeOptimizer3()
pg.initializeOnlineOptimization()
cam = camera.Camera((432.0, 432.0, 0.088981018518518529, 313.78210000000001,
313.78210000000001, 220.40700000000001))
#vo = VisualOdometer(cam)
vo = VisualOdometer(cam,
scavenge=False,
feature_detector=FeatureDetectorFast(),
descriptor_scheme=DescriptorSchemeCalonder())
def pg_constraint(pg, a, b, pose, inf):
pg.addIncrementalEdge(a, b, pose.xform(0, 0, 0), pose.euler(), inf)
def newpose(id):
xyz, euler = pg.vertex(id)
return from_xyz_euler(xyz, euler)
def mk_covar(xyz, rp, yaw):
return (1.0 / math.sqrt(xyz), 1.0 / math.sqrt(xyz), 1.0 / math.sqrt(xyz),
1.0 / math.sqrt(rp), 1.0 / math.sqrt(rp), 1.0 / math.sqrt(yaw))
def __setstate__(self, st):
self.__init__(DescriptorSchemeCalonder())
(self.cameras, self.pics) = st
for p in self.pics:
self.vt.add(None, p[1].descriptors)