def viewer_main(modelfile=None,
config='',
zoom=False,
server=False,
response='pickle'):
experiment = Experiment(zoom=zoom)
if modelfile:
experiment.execute('loadmodel %s' % modelfile)
if not server:
experiment.execute('loadconfig %s' % config)
experiment.start()
if server:
receiver = Thread(target=receive, args=(experiment, response))
receiver.start()
experiment.join()
def __init__(self, model_file, lens_lut, numc=1, vis=False):
"""
Constructor.
@param model_file: The YAML file to load the camera model.
@type model_file: C{str}
@param lens_list: List of lenses for optimization.
@type lens_list: C{list} of C{dict}
@param numc: The number of cameras to select.
@type numc: C{int}
@param vis: Enable visualization.
@type vis: C{bool}
"""
csv_dict, field_names = read_csv(lens_lut)
self.lens_dict = csv_dict
self.numc = numc
self.vis = vis
# setup the camera model.
self.exp = Experiment(zoom=False)
self.loadmodel(model_file)
self.exp.execute('loadconfig %s' % '')
# Some shortcuts.
self.model = self.exp.model
self.cam = self.exp.model['C']
self.laser = self.exp.model['L']
self.tasks = self.exp.tasks
self.task_par = self.tasks['T'].params
self.cam_par = self.model['C'].params
# start the experiment.
if self.vis:
self.exp.start()
def load_model(model, number):
ex = Experiment()
ex.execute('loadmodel %s' % model)
ex.execute('loadconfig')
# Some shortcuts.
cam_par = ex.model['C'].params
cam_primitives = ex.model['C'].primitives
cam_triangles = ex.model['C'].triangles
del_cameras(ex)
cameras = []
for i in range(number):
name = 'C'+str(i)
cameras.append(name)
ex.model[name] = Camera(name, cam_par, primitives=cam_primitives, \
triangles=cam_triangles)
ex.model[name].visualize()
return ex, cameras
def load_model(modelfile, cameras, fnumber):
ex = Experiment()
ex.execute('loadmodel %s' % modelfile)
ex.execute('loadconfig')
# situate cameras on a side of the laser
for camera in cameras:
if ex.model[camera[0]].pose.T.y < \
ex.model[ex.model.active_laser].pose.T.y:
ex.model[camera[0]].negative_side = True
else:
ex.model[camera[0]].negative_side = False
# disable unspecified cameras
for camera in ex.model.cameras:
if not camera in [c[0] for c in cameras]:
ex.model[camera].active = False
# load lens lookup tables
lut = []
for camera in cameras:
lut.append(LensLUT(camera[1], fnumber))
return ex, lut
def load_model(modelfile, cameras, fnumber, visualize=False):
if visualize:
ex = Experiment()
ex.execute('loadmodel %s' % modelfile)
ex.execute('loadconfig')
else:
ex = DummyExperiment()
ex.model, ex.tasks = YAMLParser(modelfile).experiment
# situate cameras on a side of the laser
for camera in cameras:
if ex.model[camera[0]].pose.T.y < \
ex.model[ex.model.active_laser].pose.T.y:
ex.model[camera[0]].negative_side = True
else:
ex.model[camera[0]].negative_side = False
# disable unspecified cameras
for camera in ex.model.cameras:
if not camera in [c[0] for c in cameras]:
ex.model[camera].active = False
# load lens lookup tables
lut = []
for camera in cameras:
lut.append(LensLUT(camera[1], fnumber))
return ex, lut
class ScottMethod(object):
"""
Scott's viewpoint selection method and experiment class.
"""
def __init__(self, model_file, lens_lut, numc=1, vis=False):
"""
Constructor.
@param model_file: The YAML file to load the camera model.
@type model_file: C{str}
@param lens_list: List of lenses for optimization.
@type lens_list: C{list} of C{dict}
@param numc: The number of cameras to select.
@type numc: C{int}
@param vis: Enable visualization.
@type vis: C{bool}
"""
csv_dict, field_names = read_csv(lens_lut)
self.lens_dict = csv_dict
self.numc = numc
self.vis = vis
# setup the camera model.
self.exp = Experiment(zoom=False)
self.loadmodel(model_file)
self.exp.execute('loadconfig %s' % '')
# Some shortcuts.
self.model = self.exp.model
self.cam = self.exp.model['C']
self.laser = self.exp.model['L']
self.tasks = self.exp.tasks
self.task_par = self.tasks['T'].params
self.cam_par = self.model['C'].params
# start the experiment.
if self.vis:
self.exp.start()
def run(self):
"""
The main experiment.
"""
# Generate the scene point from the scene directly.
scene_points = self.gen_scene_points(self.model['CAD'].faces, pi/2)
scene_points_c = PointCache()
for point in scene_points:
scene_points_c[point] = 1.0
self.tasks['T'] = ScottTask(self.task_par, scene_points_c)
# Visualize scene points.
if self.vis:
self.tasks['T'].visualize()
self.tasks['T'].update_visualization()
# Generate the viewpoint solution space.
view_points, view_point_poses = self.gen_view_points(scene_points)
# Visualize view points.
if self.vis:
view_points_c = PointCache()
for point in view_points:
view_points_c[point] = 1.0
view_points_c.visualize()
# Generate the measurability matrix.
m_matrix = self.gen_visual_matrix(view_points, view_point_poses, \
scene_points, "CAD")
# Select the viewpoints with the highest coverage from
# the measurability matrix.
resindex = self.select_viewpoints(m_matrix)
viewers = []
for i in resindex:
viewpoint = view_point_poses[i]
# Visualize the results.
self.cam.pose = viewpoint
if self.vis:
self.model.update_visualization()
x = raw_input()
viewers.append(viewpoint)
#print viewpoint
#x = raw_input()
self.save_network('results/result.yaml', viewers)
def loadmodel(self, model_file):
"""\
Load a model from a YAML file.
@param model_file: The YAML file to load the camera model.
@type model_file: C{str}
"""
self.exp.execute('clear %s' % '')
self.exp.execute('modify %s' % '')
self.exp.execute('cameraview %s' % '')
self.exp.execute('cameranames %s' % '')
self.exp.execute('hidetasks %s' % '')
for sceneobject in self.exp.model:
self.exp.model[sceneobject].visible = False
for triangle in self.exp.model[sceneobject].triangles:
triangle.visible = False
self.exp.model, self.exp.tasks = ScottParser(model_file, modeltypes).experiment
self.exp.model.visualize()
self.exp.display.select()
def update_camera(self, index):
"""
Update the camera parameters.
@param index: The location of the camera parameters in the parameter's list.
@type index: C{int}
"""
A = self.lens_dict['f'][index] / self.lens_dict['stop'][index]
o = [self.lens_dict['ou'][index], self.lens_dict['ov'][index]]
self.cam.setparam('A', A)
self.cam.setparam('f', self.lens_dict['f'][index])
self.cam.setparam('o', o)
self.cam.setparam('zS', self.lens_dict['zS'][index])
def gen_scene_points(self, triangles, threshold):
"""\
Generate the directional points that model the task from the list of
triangles of the associated CAD file as given by the raw triangle
representation.
@param triangles: The collection of triangles in the model representation.
@type triangles: C{list} of L{adolphus.geometry.Triangle}
@param threshold: The inclination angle threshold.
@type threshold: C{float}
@return: A list of directional points.
@rtype: C{list} of L{DirectionalPoint}
"""
points = []
for triangle in triangles:
average = avg_points(triangle.vertices)
angles = triangle.normal_angles()
if angles[0] > threshold:
continue
points.append(DirectionalPoint(average.x, average.y, average.z, \
angles[0], angles[1]))
return points
def gen_view_points(self, scene_points, mode='flat'):
"""\
Generate the solution space as the list of viewpoints.
@param scene_points: The list of scene points in the task model.
@type scene_points: C{list}
@param mode: Select whether to position all cameras flat, vertical, or
alternate between flat and vertical. Options: C{flat}, C{alternate},
C{vertical}
@type mode: C{str}
@return: A pair of lists containing the viewpoints and their poses.
@rtype: C{list}
"""
view_points = []
view_point_poses = []
flat = True
# Standoff calculation as described in Scott 2009, Section 4.4.3.
f_d = 1.02
R_o = max(self.cam.zres(self.task_par['res_max'][1]), \
self.cam.zc(self.task_par['blur_max'][1] * \
min(self.cam.getparam('s')))[0])
e = 1e9
for zS in self.lens_dict['zS']:
if abs(zS - (f_d * R_o)) < e:
e = abs(zS - (f_d * R_o))
index = self.lens_dict['zS'].index(zS)
# Updating the camera after computing the camera standoff distance simulates
# the action of focusing the lens.
self.update_camera(index)
# This implementation assumes that the camera is mounted on the laser
# and it is in fact the laser that moves.
z_lim = [max(self.cam.zres(self.task_par['res_max'][1]),
self.cam.zc(self.task_par['blur_max'][1] * \
min(self.cam_par['s']))[0]),
min(self.cam.zres(self.task_par['res_min'][1]),
self.cam.zc(self.task_par['blur_max'][1] * \
min(self.cam_par['s']))[1])]
cam_standoff = z_lim[1] * 0.9
self.laser.mount = self.cam
T = Point(0, cam_standoff * tan(0.7853), 0)
R = Rotation.from_euler('zyx', (Angle(-0.7853), Angle(0), Angle(0)))
self.laser.set_relative_pose(Pose(T, R))
# The camera standoff.
standoff = cam_standoff
for point in scene_points:
x = point.x + (standoff * sin(point.rho) * cos(point.eta))
y = point.y + (standoff * sin(point.rho) * sin(point.eta))
z = point.z + (standoff * cos(point.rho))
rho = point.rho + pi
eta = point.eta
point = DirectionalPoint(x, y, z, rho, eta)
view_points.append(point)
normal = point.direction_unit()
angle = Point(0, 0, 1).angle(normal)
axis = Point(0, 0, 1).cross(normal)
pose = Pose(Point(x, y, z), Rotation.from_axis_angle(angle, axis))
angles = pose.R.to_euler_zyx()
if mode == 'flat':
if y >= 0:
new_angles = (angles[0], angles[1], Angle(pi))
elif y < 0:
new_angles = (angles[0], angles[1], Angle(0))
elif mode == 'vertical':
new_angles = (angles[0], angles[1], Angle(0))
elif mode == 'alternate':
if flat:
new_angles = (angles[0], angles[1], Angle(1.5707))
flat = False
elif not flat:
new_angles = (angles[0], angles[1], Angle(0))
flat = True
new_pose = Pose(Point(x,y,z), Rotation.from_euler('zyx', new_angles))
view_point_poses.append(new_pose)
return view_points, view_point_poses
def gen_visual_matrix(self, view_points, view_point_poses, scene_points, \
sceneobj=None):
"""\
Compute the measurability matrix.
@param view_points: The list of viewpoints in the solution space.
@type view_points: C{list}
@param view_point_poses: The list of poses of the viewpoints.
@type view_point_poses: C{list}
@param scene_points: the list of scene points in the task model.
@type scene_points: C{list}
@param sceneobj: The name of the object containing the occlusion triangles.
@type sceneobj: C{str}
@return: the visual matrix whose entries represent coverage strength.
@rtype: L{numpy.array}
"""
if sceneobj:
solid = self.model[sceneobj]
f = len(solid.faces)
else:
solid = None
n = len(scene_points)
v = len(view_points)
vis_matrix = zeros((n, v))
for i in range(n):
for j in range(v):
if solid:
occluded = False
for k in xrange(f):
if solid.faces[k].intersection(scene_points[i], \
view_points[j], True):
occluded = True
break
if occluded:
continue
if i == j:
vis_matrix[i,j] = 1.0
continue
self.cam.pose = view_point_poses[j]
try:
strength = self.cam.strength(scene_points[i], \
self.laser.pose, self.task_par)
except ValueError:
strength = 0.0
vis_matrix[i,j] = strength
return vis_matrix
def select_viewpoints(self, matrix):
"""\
Perform set viewpoint selection on the measurability matrix by computing a
Figure of Merit for each viewpoint as the total of covered points and selecting
the viewpoint with the highest FOM. See Scott 2002, Section 4.4.2.
@param matrix: The measurability matrix.
@type matrix: L{numpy.array}
@return: The selected viewpoint.
@rtype: C{list} of C{int}
"""
index_res = []
size = len(matrix)
for dog in range(size):
coverage = sum(matrix)
best = 0
index = 0
for i in range(size):
if coverage[i] > best:
best = coverage[i]
index = i
index_res.append(index)
if dog == (self.numc - 1) or best == 0:
return index_res
for duck in range(size):
if matrix[duck, index] != 0:
for rowduck in range(size):
matrix[duck,rowduck] = 0
return index_res
def save_network(self, result_file, network):
"""\
Save the sensor deployment into Adolphus' yaml format.
@param result_file: The name of the file.
@type result_file: C{str}
@param network: The poses of the camera network.
@type network: C{list} of L{adolphus.geometry.Pose}
"""
with open(result_file, 'w') as result:
result.write("model:\n")
result.write(" name: Experiment\n\n")
result.write(" cameras:\n")
i = 0
for pose in network:
result.write(" - name: 'C%d'\n" %i)
result.write(" sprites: ['cameras/prosilicaec1350.yaml', 'lenses/computarm3z1228cmp.yaml']\n")
result.write(" A: %.2f\n" %self.cam_par['A'])
result.write(" f: %d\n" %self.cam_par['f'])
result.write(" s: %.5f\n" %self.cam_par['s'][0])
result.write(" o: [%d, %d]\n" \
%(self.cam_par['o'][0],self.cam_par['o'][1]))
result.write(" dim: [%d, %d]\n" \
%(self.cam_par['dim'][0],self.cam_par['dim'][1]))
result.write(" zS: %.1f\n" %self.cam_par['zS'])
result.write(" pose:\n")
T = pose.T
R = pose.R.Q
result.write(" T: [" +
str(T.x) + ", " + str(T.y) + ", " + str(T.z) + "]\n" +
" R: [" +
str(R.a) + ", [" + str(R.v.x) + ", " + str(R.v.y) + ", " + \
str(R.v.z) + "]]\n")
result.write(" Rformat: quaternion\n\n")
i += 1
result.write("tasks:\n")
result.write(" - name: 'T'\n")
result.write(" parameters:\n")
result.write(" res_min: [%.1f, %.1f]\n" \
%(self.task_par['res_min'][0], self.task_par['res_min'][1]))
result.write(" blur_max: [%.1f, %.1f]\n" \
%(self.task_par['blur_max'][0], self.task_par['blur_max'][1]))
result.write(" angle_max: [%.4f, %.4f]\n" \
%(self.task_par['angle_max'][0], self.task_par['angle_max'][1]))
result.write(" points:\n")
result.write(" - [0, 0, 0]\n\n")
parser.add_option('-J', '--jitter', dest='jitter', action='store',
type='int', default=0, help='jitter threshold in frames')
parser.add_option('-c', '--conf', dest='conf', default=None,
help='custom configuration file to load')
parser.add_option('-z', '--zoom', dest='zoom', default=False,
action='store_true', help='disable camera view and use visual zoom')
parser.add_option('-g', '--graph', dest='graph', action='store',
default=None, help='pickle of vision graph')
parser.add_option('-r', '--robotpath', dest='robotpath', action='store',
default=None, help='robot positions file')
parser.add_option('-w', '--wtolerance', dest='w', action='store',
type='float', default=0.1, help='jitter tolerance')
opts, args = parser.parse_args()
modelfile, targetobj, targetrm = args[:3]
# load model
experiment = Experiment(zoom=opts.zoom)
#experiment.add_display()
experiment.execute('loadmodel %s' % modelfile)
experiment.execute('loadconfig %s' % opts.conf)
# compute vision graph
if opts.graph and os.path.exists(opts.graph):
vision_graph = pickle.load(open(opts.graph, 'r'))
else:
try:
vision_graph = experiment.model.coverage_hypergraph(\
experiment.relevance_models[args[3]], K=2)
except IndexError:
vision_graph = None
if opts.graph:
pickle.dump(vision_graph, open(opts.graph, 'w'))
# set up network socket
from adolphus.interface import Experiment
ex = Experiment(zoom=False)
ex.execute('loadmodel %s' % 'scene.yaml')
ex.execute('loadconfig %s' % '')
ex.start()
parser.add_argument('-f', '--floor', dest='floor', type=str,
default="../models/floor.dae")
parser.add_argument('-w', '--walls', dest='walls', type=str,
default="../models/walls.dae")
parser.add_argument('-m', '--mounts', dest='mounts', type=str,
default="../models/mounts.dae")
parser.add_argument('-p', '--path', dest='path', type=str,
default="path.pts")
parser.add_argument('-mo', '--model', dest='model', type=str, default="empty.yaml")
parser.add_argument('-i', '--interpolate', dest='interpolate',
type=int, default=100, help='interpolation pitch')
parser.add_argument('-v', dest='vis', action='store_true', default=False)
args = parser.parse_args()
# Load the experiment.
ex = Experiment()
ex.execute('loadmodel %s' % args.model)
ex.execute('loadconfig')
# Some shortcuts.
cam_par = ex.model['C'].params
cam_primitives = ex.model['C'].primitives
cam_triangles = ex.model['C'].triangles
task_par = ex.tasks['T'].params
# Load the parts.
ex.model['floor'] = Solid(args.floor, 'floor')
ex.model['walls'] = Solid(args.walls, 'walls')
ex.model['mounts'] = Solid(args.mounts, 'mounts')
# Set occlusion triangles.
occ_triangles = []
for item in ['walls', 'mounts']:
for t in ex.model[item].faces:
dest='threshold',
type=float,
default=0.0,
help='hysteresis threshold')
parser.add_argument('pathfile', help='path waypoint file')
args = parser.parse_args()
# Load path waypoints.
print('Loading path waypoints...')
points = []
with open(args.pathfile, 'r') as pf:
for line in pf.readlines():
points.append(Point(*[float(s) for s in line.rstrip().split(',')]))
path = interpolate_points(points)
# Set up displays and load the model.
print('Loading model...')
ex = Experiment(zoom=True)
ex.add_display()
ex.display.autoscale = True
ex.display.userspin = False
ex.display.forward = (0, 0, -1)
ex.display.up = (0, 1, 0)
ex.execute('loadmodel follow.yaml')
ex.execute('loadconfig')
# Compute the vision graph.
if os.path.exists('vgraph.pickle'):
print('Loading vision graph...')
vision_graph = pickle.load(open('vgraph.pickle', 'r'))
else:
try:
print('Computing vision graph...')
vision_graph = ex.model.coverage_hypergraph(ex.tasks['scene'], K=2)
parser.add_argument('-i', '--interpolate', dest='interpolate',
type=int, default=100, help='interpolation pitch')
parser.add_argument('-t', '--threshold', dest='threshold', type=float,
default=0.0, help='hysteresis threshold')
parser.add_argument('pathfile', help='path waypoint file')
args = parser.parse_args()
# Load path waypoints.
print('Loading path waypoints...')
points = []
with open(args.pathfile, 'r') as pf:
for line in pf.readlines():
points.append(Point(*[float(s) for s in line.rstrip().split(',')]))
path = interpolate_points(points)
# Set up displays and load the model.
print('Loading model...')
ex = Experiment(zoom=True)
ex.add_display()
ex.display.autoscale = True
ex.display.userspin = False
ex.display.forward = (0, 0, -1)
ex.display.up = (0, 1, 0)
ex.execute('loadmodel follow.yaml')
ex.execute('loadconfig')
# Compute the vision graph.
if os.path.exists('vgraph.pickle'):
print('Loading vision graph...')
vision_graph = pickle.load(open('vgraph.pickle', 'r'))
else:
try:
print('Computing vision graph...')
vision_graph = ex.model.coverage_hypergraph(ex.tasks['scene'], K=2)
