def test_pymvg_file_in_docs():
pymvg_src_path = pymvg.__file__
pymvg_base = os.path.split(pymvg_src_path)[0]
pymvg_src_dir = os.path.join(pymvg_base, '..')
fname = os.path.join(pymvg_src_dir, 'docs', 'source',
'pymvg_camsystem_example.json')
system = MultiCameraSystem.from_pymvg_file(fname)
def main():
out_fname = sys.argv[1]
system1 = make_default_system()
system1.save_to_pymvg_file( out_fname )
# just make sure we can actually read it!
system2 = MultiCameraSystem.from_pymvg_file( out_fname )
assert system1==system2
def test_roundtrip_to_pymvg_file():
system1 = make_default_system()
fname = tempfile.mktemp(suffix='.json')
system1.save_to_pymvg_file( fname )
try:
system2 = MultiCameraSystem.from_pymvg_file( fname )
assert system1==system2
finally:
os.unlink( fname )
def test_roundtrip_to_pymvg_file():
system1 = make_default_system()
fname = tempfile.mktemp(suffix='.json')
system1.save_to_pymvg_file(fname)
try:
system2 = MultiCameraSystem.from_pymvg_file(fname)
assert system1 == system2
finally:
os.unlink(fname)
def main(args):
if not path.exists(args.data):
raise ValueError("Data path does not exist!")
# Load calibration:
cam_path = path.join(args.data, "camera_system_aligned.json")
camera_system = MultiCameraSystem.from_pymvg_file(cam_path)
# Load data:
data = load_data(args.data, camera_system)
# Filter by area:
data = data[data.area > args.area_filter]
# Select a range:
if args.frame_range is None:
a, b = 0, data.index.levels[1].max()
else:
a, b = args.frame_range
data = data.loc[pd.IndexSlice[:, a:b], :]
data = data.reset_index().set_index(["camera_id",
"frame_number"]).sort_index()
# Set up reconstruction:
rec = FastSeqH(camera_system, minimum_tracks=args.minimum_tracks)
# Run:
output = multi_reconstruct(data, rec, args).reset_index().set_index(
["frame_number", "point_id"])
# Save:
output_directory = path.join(args.data, "reconstruction")
if not path.exists(output_directory):
os.mkdir(output_directory)
output_path = path.join(output_directory, "points.csv")
output.to_csv(output_path)
def test_pymvg_file_in_docs():
pymvg_src_path = pymvg.__file__
pymvg_base = os.path.split(pymvg_src_path)[0]
pymvg_src_dir = os.path.join(pymvg_base,'..')
fname = os.path.join( pymvg_src_dir, 'docs', 'source', 'pymvg_camsystem_example.json')
system = MultiCameraSystem.from_pymvg_file( fname )
new_cams = []
for name in old.get_names():
orig_cam = old.get_camera(name)
new_cam = orig_cam.get_aligned_camera(s, R, T)
new_cams.append(new_cam)
return MultiCameraSystem(new_cams)
if __name__ == "__main__":
images, camids = take_images()
points = run_point_select(camids, images)
# np.save("points", points)
camera_system = MultiCameraSystem.from_pymvg_file(path.join(BASE_CALIBRATION, "camera_system.json"))
# points = np.load("points.npy")
final_points = triangulate_points(camera_system, points)
sorted_points = identify_points(final_points)
plot_reprojection(camera_system, images, sorted_points)
s, R, T = align.estsimt(sorted_points.T, CALIBRATED_COORDINATES.T)
new_system = transform_system(camera_system, s, R, T)
new_system.save_to_pymvg_file(path.join(BASE_CALIBRATION, "camera_system_aligned.json"))
# DEBUGGING:
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
def _build_test_camera(**kwargs):
o = Bunch(**kwargs)
if not kwargs.get('at_origin',False):
translation = geometry_msgs.msg.Point()
translation.x = 0.273485679077
translation.y = 0.0707310128808
translation.z = 0.0877802104531
rotation = geometry_msgs.msg.Quaternion()
rotation.x = 0.309377331102
rotation.y = 0.600893485738
rotation.z = 0.644637681813
rotation.w = 0.357288321925
else:
translation = geometry_msgs.msg.Point()
translation.x = 0.0
translation.y = 0.0
translation.z = 0.0
rotation = geometry_msgs.msg.Quaternion()
rotation.x = 0.0
rotation.y = 0.0
rotation.z = 0.0
rotation.w = 1.0
if 'from_file' in kwargs:
if kwargs['type']=='ros':
cam = CameraModel.load_camera_from_file( fname=kwargs['filename'],
extrinsics_required=False )
i = cam.get_intrinsics_as_bunch()
cam = CameraModel._from_parts(
translation=point_msg_to_tuple(translation),
rotation=parse_rotation_msg(rotation),
intrinsics=i,
name='cam',
)
elif kwargs['type']=='pymvg':
del translation
del rotation
system = MultiCameraSystem.from_pymvg_file(fname=kwargs['filename'])
names = system.get_names()
names.sort()
name = names[0] # get first camera from system
cam = system._cameras[name]
rotation = cam.get_extrinsics_as_bunch().rotation
translation = cam.get_extrinsics_as_bunch().translation
else:
raise ValueError('unknown file type')
else:
if o.ROS_test_data:
i = sensor_msgs.msg.CameraInfo()
# these are from image_geometry ROS package in the utest.cpp file
i.height = 480
i.width = 640
i.distortion_model = 'plumb_bob'
i.D = [-0.363528858080088, 0.16117037733986861, -8.1109585007538829e-05, -0.00044776712298447841, 0.0]
i.K = [430.15433020105519, 0.0, 311.71339830549732,
0.0, 430.60920415473657, 221.06824942698509,
0.0, 0.0, 1.0]
i.R = [0.99806560714807102, 0.0068562422224214027, 0.061790256276695904,
-0.0067522959054715113, 0.99997541519165112, -0.0018909025066874664,
-0.061801701660692349, 0.0014700186639396652, 0.99808736527268516]
i.P = [295.53402059708782, 0.0, 285.55760765075684, 0.0,
0.0, 295.53402059708782, 223.29617881774902, 0.0,
0.0, 0.0, 1.0, 0.0]
else:
i = sensor_msgs.msg.CameraInfo()
i.height = 494
i.width = 659
i.distortion_model = 'plumb_bob'
i.D = [-0.34146457767225, 0.196070795764995, 0.000548988393912233, 0.000657058395082583, -0.0828776806503243]
i.K = [516.881868241566, 0.0, 333.090936517613, 0.0, 517.201263180996, 231.526036849886, 0.0, 0.0, 1.0]
i.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
i.P = [442.17529296875, 0.0, 334.589001099812, 0.0, 0.0, 474.757141113281, 228.646131377705, 0.0, 0.0, 0.0, 1.0, 0.0]
cam = CameraModel._from_parts(
translation=point_msg_to_tuple(translation),
rotation=parse_rotation_msg(rotation),
intrinsics=i,
name='cam',
)
if kwargs.get('flipped',False):
cam = cam.get_flipped_camera()
if kwargs.get('get_input_data',False):
return dict(cam=cam,
translation=translation,
rotation=rotation,
)
return cam
def _build_test_camera(**kwargs):
o = Bunch(**kwargs)
if not kwargs.get('at_origin',False):
translation = geometry_msgs.msg.Point()
translation.x = 0.273485679077
translation.y = 0.0707310128808
translation.z = 0.0877802104531
rotation = geometry_msgs.msg.Quaternion()
rotation.x = 0.309377331102
rotation.y = 0.600893485738
rotation.z = 0.644637681813
rotation.w = 0.357288321925
else:
translation = geometry_msgs.msg.Point()
translation.x = 0.0
translation.y = 0.0
translation.z = 0.0
rotation = geometry_msgs.msg.Quaternion()
rotation.x = 0.0
rotation.y = 0.0
rotation.z = 0.0
rotation.w = 1.0
if 'from_file' in kwargs:
if kwargs['type']=='ros':
cam = CameraModel.load_camera_from_file( fname=kwargs['filename'],
extrinsics_required=False )
i = cam.get_intrinsics_as_bunch()
cam = CameraModel._from_parts(
translation=point_msg_to_tuple(translation),
rotation=parse_rotation_msg(rotation),
intrinsics=i,
name='cam',
)
elif kwargs['type']=='pymvg':
del translation
del rotation
system = MultiCameraSystem.from_pymvg_file(fname=kwargs['filename'])
names = system.get_names()
names.sort()
name = names[0] # get first camera from system
cam = system._cameras[name]
rotation = cam.get_extrinsics_as_bunch().rotation
translation = cam.get_extrinsics_as_bunch().translation
else:
raise ValueError('unknown file type')
else:
if o.ROS_test_data:
i = sensor_msgs.msg.CameraInfo()
# these are from image_geometry ROS package in the utest.cpp file
i.height = 480
i.width = 640
i.distortion_model = 'plumb_bob'
i.D = [-0.363528858080088, 0.16117037733986861, -8.1109585007538829e-05, -0.00044776712298447841, 0.0]
i.K = [430.15433020105519, 0.0, 311.71339830549732,
0.0, 430.60920415473657, 221.06824942698509,
0.0, 0.0, 1.0]
i.R = [0.99806560714807102, 0.0068562422224214027, 0.061790256276695904,
-0.0067522959054715113, 0.99997541519165112, -0.0018909025066874664,
-0.061801701660692349, 0.0014700186639396652, 0.99808736527268516]
i.P = [295.53402059708782, 0.0, 285.55760765075684, 0.0,
0.0, 295.53402059708782, 223.29617881774902, 0.0,
0.0, 0.0, 1.0, 0.0]
else:
i = sensor_msgs.msg.CameraInfo()
i.height = 494
i.width = 659
i.distortion_model = 'plumb_bob'
i.D = [-0.34146457767225, 0.196070795764995, 0.000548988393912233, 0.000657058395082583, -0.0828776806503243]
i.K = [516.881868241566, 0.0, 333.090936517613, 0.0, 517.201263180996, 231.526036849886, 0.0, 0.0, 1.0]
i.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
i.P = [442.17529296875, 0.0, 334.589001099812, 0.0, 0.0, 474.757141113281, 228.646131377705, 0.0, 0.0, 0.0, 1.0, 0.0]
cam = CameraModel._from_parts(
translation=point_msg_to_tuple(translation),
rotation=parse_rotation_msg(rotation),
intrinsics=i,
name='cam',
)
if kwargs.get('flipped',False):
cam = cam.get_flipped_camera()
if kwargs.get('get_input_data',False):
return dict(cam=cam,
translation=translation,
rotation=rotation,
)
return cam
print cam.get_camera_info()
cam.set_property_values(cam_settings[sn])
cam.set_format_configuration(**cam_settings[sn]["f7"])
cam.set_trigger(True)
cam.start_capture()
time.sleep(0.5)
return cam
if __name__ == "__main__":
# Load Camera System:
camsys = MultiCameraSystem.from_pymvg_file(BASE_CALIB)
# Set up sync:
sync = FrameSync()
sync.set_framerate(30, duty_cycle=0.5)
sync.start()
time.sleep(1)
cams = [set_up_camera(sn) for sn in cam_settings.keys()]
camid = [cam.get_camera_info()["serial_number"] for cam in cams]
cv2.namedWindow("viewer", cv2.WINDOW_AUTOSIZE)
starttime = time.time()
update_rate = 1.0
