import numpy as np
from pymvg.multi_camera_system import build_example_system
from pymvg.extern.ros.ros_publisher import ROSPublisher
import roslib # ROS is required for this example. See http://ros.org
roslib.load_manifest('rospy')
import rospy
cam_pubs = []
rospy.init_node("publish_sample_cameras")
n=6
z=5.0
system = build_example_system(n=n,z=z)
for name in system.get_names():
cam_pubs.append(ROSPublisher(system.get_camera(name)))
rospy.spin()
def test_build_example_system():
for n in range(2,100,5):
system = build_example_system(n=n)
assert n==len(system.get_names())
from __future__ import print_function
from pymvg.multi_camera_system import build_example_system
import numpy as np
system = build_example_system()
point_3d_expected = np.array([1.0, 2.0, 3.0])
print('Original: %r'%(point_3d_expected,))
# find perfect 2D projection of this original 3D point
data = []
for camera_name in system.get_names():
this_pt2d = system.find2d(camera_name,point_3d_expected)
data.append( (camera_name,this_pt2d) )
print('%r: %r'%(camera_name,this_pt2d))
# now triangulate 3D point
point_3d_actual = system.find3d(data)
print('Result ----> %r'%(point_3d_actual,))
from pymvg.plot_utils import plot_camera
from pymvg.multi_camera_system import build_example_system
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import numpy as np
n = 6
z = 5.0
system = build_example_system(n=n, z=z)
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
for name in system.get_names():
plot_camera(ax, system.get_camera(name), scale=z / 5.0)
if 1:
# put some points to force mpl's view dimensions
pts = np.array([[0, 0, 0], [2 * n, 2 * z, 2 * z]])
ax.plot(pts[:, 0], pts[:, 1], pts[:, 2], 'k.')
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_zlabel('z')
plt.show()
def test_build_example_system():
for n in range(2, 100, 5):
system = build_example_system(n=n)
assert n == len(system.get_names())
from __future__ import print_function
from pymvg.multi_camera_system import build_example_system
import numpy as np
system = build_example_system()
point_3d_expected = np.array([1.0, 2.0, 3.0])
print('Original: %r' % (point_3d_expected, ))
# find perfect 2D projection of this original 3D point
data = []
for camera_name in system.get_names():
this_pt2d = system.find2d(camera_name, point_3d_expected)
data.append((camera_name, this_pt2d))
print('%r: %r' % (camera_name, this_pt2d))
# now triangulate 3D point
point_3d_actual = system.find3d(data)
print('Result ----> %r' % (point_3d_actual, ))
