def check_bagfile_roundtrip(cam_opts):
"""check that roundtrip of camera model to/from a bagfile works"""
cam = _build_test_camera(**cam_opts)
fname = tempfile.mktemp(suffix='.bag')
try:
with open(fname,mode='wb') as fd:
cam.save_to_bagfile(fd, roslib)
with open(fname,mode='r') as fd:
bag = rosbag.Bag(fd, 'r')
cam2 = CameraModel.load_camera_from_opened_bagfile( bag )
finally:
os.unlink(fname)
verts = np.array([[ 0.042306, 0.015338, 0.036328],
[ 0.03323, 0.030344, 0.041542],
[ 0.03323, 0.030344, 0.041542],
[ 0.03323, 0.030344, 0.041542],
[ 0.036396, 0.026464, 0.052408]])
expected = cam.project_3d_to_pixel(verts)
actual = cam2.project_3d_to_pixel(verts)
assert np.allclose( expected, actual )
