def handle_raw_stereo(self, msg):
size = (msg.left_info.width, msg.left_info.height)
if self.vo == None:
cam = camera.StereoCamera(msg.right_info)
self.vo = VisualOdometer(cam,
scavenge=False,
inlier_error_threshold=3.0,
sba=None,
inlier_thresh=100,
position_keypoint_thresh=0.2,
angle_keypoint_thresh=0.15)
pair = [imgAdapted(i, size) for i in [msg.left_image, msg.right_image]]
af = SparseStereoFrame(pair[0],
pair[1],
feature_detector=self.fd,
descriptor_scheme=self.ds)
pose = self.vo.handle_frame(af)
p = deprecated_msgs.msg.VOPose()
p.inliers = self.vo.inl
# XXX - remove after camera sets frame_id
p.header = roslib.msg.Header(0, msg.header.stamp, "stereo_link")
p.pose = geometry_msgs.msg.Pose(
geometry_msgs.msg.Point(*pose.xform(0, 0, 0)),
geometry_msgs.msg.Quaternion(*pose.quaternion()))
self.pub_vo.publish(p)
# turn this on to skip every other frame
flag_15Hz = 0
# limits of file images
start, end = 941, 1000
start, end = 0, 27000
f1start, f1end = 0, 3000
f2start, f2end = 20000, 23000
for topic, msg, t in rosrecord.logplayer(filename):
if rospy.is_shutdown():
break
if topic == "/dcam/raw_stereo":
if not cam:
cam = camera.StereoCamera(msg.right_info)
vo = VisualOdometer(cam, scavenge = False, feature_detector = FeatureDetectorFast(), \
inlier_error_threshold = 3.0, sba = None, \
inlier_thresh = 100, \
position_keypoint_thresh = 0.2, angle_keypoint_thresh = 0.15)
vo_x = []
vo_y = []
vo_u = []
vo_v = []
trajectory = []
frames1 = []
frames2 = []
if framecounter == end:
break
has_moved = False
# set high for getting different views