def __init__(self, world):
self.world = world
self.current_bin = "A"
self.bin_state = {
"A": {"done": False, "contents": []},
"B": {"done": False, "contents": []},
"C": {"done": False, "contents": []},
"D": {"done": False, "contents": []},
"E": {"done": False, "contents": []},
"F": {"done": False, "contents": []},
"G": {"done": False, "contents": []},
"H": {"done": False, "contents": []},
"I": {"done": False, "contents": []},
"J": {"done": False, "contents": []},
"K": {"done": False, "contents": []},
"L": {"done": False, "contents": []},
}
self.robotModel = world.robot(0)
self.state = INITIAL_STATE
self.config = self.robotModel.getConfig()
self.left_arm_links = [self.robotModel.link(i) for i in LEFT_ARM_LINK_NAMES]
self.right_arm_links = [self.robotModel.link(i) for i in RIGHT_ARM_LINK_NAMES]
id_to_index = dict([(self.robotModel.link(i).getID(), i) for i in range(self.robotModel.numLinks())])
self.left_arm_indices = [id_to_index[i.getID()] for i in self.left_arm_links]
self.right_arm_indices = [id_to_index[i.getID()] for i in self.right_arm_links]
self.Tcamera = se3.identity()
self.Tvacuum = se3.identity()
self.calibratedCameraXform = RIGHT_F200_CAMERA_CALIBRATED_XFORM
self.object_com = [0, 0, 0]
self.points1 = []
self.points2 = []
self.vacuumPc = Geometry3D()
self.vacuumPc.loadFile(VACUUM_PCD_FILE)
# Set up serial
if REAL_VACUUM:
self.serial = serial.Serial()
self.serial.port = ARDUINO_SERIAL_PORT
self.serial.baudrate = 9600
self.serial.open()
if self.serial.isOpen():
self.serial.write("hello")
response = self.serial.read(self.serial.inWaiting())
self.turnOffVacuum()
# Load JSON
with open(PICK_JSON_PATH) as pick_json_file:
raw_json_data = json.load(pick_json_file)
for k in self.bin_state:
self.bin_state[k]["contents"] = raw_json_data["bin_contents"]["bin_" + k]
for my_dict in raw_json_data["work_order"]:
bin_letter = my_dict["bin"][4]
self.bin_state[bin_letter]["target"] = my_dict["item"]
self.current_bin = planning.selectBin(self.bin_state)
def __init__(self,xform = se3.identity()):
#the transformation of the gripper fingers relative to the object's
#local coordinates
self.grasp_xform = xform
#the command to be sent to the gripper's fingers to close it properly
#around this object
self.gripper_close_command = [0]
#the command to be sent to the gripper's fingers to open it back
#after grasping this object
self.gripper_open_command = [1]
self.pregrasp = se3.identity()
def __init__(self, apc_order, test=False):
# if currently a testing run
if not test:
Master.__init__(self,apc_order)
# if currently not a testing run
else:
self._create_knowledge_base()
self._load_apc_order(apc_order)
# load the .csv file in /apc/high_level_planning/...csv
load_strategy_constants("apc/high_level_planning/Strategy Optimization.csv")
# defaultdict is a "high-performance container datatype". Similar to a dictionary
# Using "list" as the argument of defaultdict
self.order_by_bin = defaultdict(list)
# sort the order item by bin
# i.e.) [ (binA,[item1, item2]), (binB,[item4,item9]), (...), ... ]
for orderitem in self.order:
self.order_by_bin[orderitem['bin']].append(orderitem['item'])
self.cache = False
print "Loading prior point clouds"
# load prior point cloud, transforms from databse for items in the knowledgeBase
if self.cache:
for bin,objects in self.knowledge_base.bin_contents.iteritems():
for o in objects:
pcd_file_name = '/tmp/'+o+'_'+bin+".pcd"
if os.path.exists(pcd_file_name):
self.knowledge_base.object_xforms[o] = se3.identity()
self.knowledge_base.object_clouds[o] = pcd.read(open(pcd_file_name))[1]
def __init__(self, world):
self.world = world
self.current_bin = 'A'
self.bin_state = {'A': {'done': False, 'contents': []},'B': {'done': False, 'contents': []}, 'C': {'done': False, 'contents': []}, 'D': {'done': False, 'contents': []}, 'E': {'done': False, 'contents': []}, 'F': {'done': False, 'contents': []}, 'G': {'done': False, 'contents': []}, 'H': {'done': False, 'contents': []}, 'I': {'done': False, 'contents': []}, 'J': {'done': False, 'contents': []}, 'K': {'done': False, 'contents': []}, 'L': {'done': False, 'contents': []}}
self.robotModel = world.robot(0)
self.state = INITIAL_STATE
self.config = self.robotModel.getConfig()
self.left_arm_links = [self.robotModel.link(i) for i in LEFT_ARM_LINK_NAMES]
self.right_arm_links = [self.robotModel.link(i) for i in RIGHT_ARM_LINK_NAMES]
id_to_index = dict([(self.robotModel.link(i).getID(),i) for i in range(self.robotModel.numLinks())])
self.left_arm_indices = [id_to_index[i.getID()] for i in self.left_arm_links]
self.right_arm_indices = [id_to_index[i.getID()] for i in self.right_arm_links]
self.Tcamera = se3.identity()
self.Tvacuum = se3.identity()
self.calibratedCameraXform = RIGHT_F200_CAMERA_CALIBRATED_XFORM
self.object_com = [0, 0, 0]
self.points1 = []
self.points2 = []
self.vacuumPc = Geometry3D()
self.vacuumPc.loadFile(VACUUM_PCD_FILE)
if REAL_SCALE:
self.scale = scale.Scale();
# Set up serial
if REAL_VACUUM:
self.serial = serial.Serial()
self.serial.port = ARDUINO_SERIAL_PORT
self.serial.baudrate = 9600
self.serial.open()
if self.serial.isOpen():
self.serial.write("hello")
response = self.serial.read(self.serial.inWaiting())
self.turnOffVacuum()
# Load JSON
with open(PICK_JSON_PATH) as pick_json_file:
raw_json_data = json.load(pick_json_file)
for k in self.bin_state:
self.bin_state[k]['contents'] = raw_json_data['bin_contents']['bin_'+k]
for my_dict in raw_json_data['work_order']:
bin_letter = my_dict['bin'][4]
self.bin_state[bin_letter]['target'] = my_dict['item']
self.current_bin = planning.selectBin(self.bin_state)
def icp(object,scene): #return se3.identity() """Computes a rotation and translation of the object to the scene using the ICP algorithm. Returns the transform (R,t) in Klamp't se3 format. """ kd = KDTree(scene) print "kd tree done" #TODO: implement me # Sample both maps to make the closest point computations faster # Compute the minimum distance between the points # Reject the outliers, for example, using a threshold # Compute the R and t matrices. The procedure can be similar as the one for the # 2D images. #ret = se3.identity() #return ret #ret[1][2]+=0.2 #ret[1][1]+=0.3 #return ret sampled = random.sample(object, 1000) threshold = 0.75 net = se3.identity() #net =[net[0], (0.2,0.05,0)] #return net sampled = [se3.apply(net, o) for o in sampled] #print object[0] for _ in xrange(10): print "start finding correspondances" correspondings = [] for i in sampled: try: (dist, idx) = kd.query(i) except: print i raise Exception() correspondings.append((i, scene[idx], dist)) correspondings.sort(key=lambda x: x[2]) correspondings = correspondings[0:int(len(correspondings)*threshold)] #print "\n".join(map(str,correspondings)) print "done finding correspondances" object_points = [i[0] for i in correspondings] scene_points = [i[1] for i in correspondings] (R,t) = one_round_icp(scene_points, object_points, 'list') #(R,t) = one_round_icp(object_points, scene_points, 'matrix') sampled = [se3.apply((R,t),i) for i in sampled] net = se3.mul((R,t), net) #print R,t #print net return net
def __init__(self, world, xforms=None, title=None):
GLRealtimeProgram.__init__(self, title or "World Visualization")
self.world = world
self.xforms = xforms or []
self.xforms += [se3.identity()]
# configure the rendering
self.clippingplanes = (0.1, 100)
logger.info("visualization ready")
def icp(object,scene):
"""Computes a rotation and translation of the object to the scene
using the ICP algorithm. Returns the transform (R,t) in Klamp't se3
format.
"""
#TODO: implement me
# Sample both maps to make the closest point computations faster
# Compute the minimum distance between the points
# Reject the outliers, for example, using a threshold
# Compute the R and t matrices. The procedure can be similar as the one for the
# 2D images.
return se3.identity()
def __init__(self, clouds, xforms=None, world=None):
GLRealtimeProgram.__init__(self, "Cloud Visualization")
self.clouds = clouds or []
self.point_lists = []
self.xforms = xforms or []
self.xforms += [se3.identity()]
self.world = world
self.point_size = 2
# configure the rendering
self.clippingplanes = (0.1, 100)
logger.info("visualization ready")
def move_to_grasp_object(self,object):
"""Sets the robot's configuration so the gripper grasps object at
one of its potential grasp locations. Might change self.active_limb
to the appropriate limb.
If successful, changes self.config and returns True.
Otherwise, does not change self.config and returns False.
"""
print
print 'Trying to grasp ' + object.info.name
grasp_transforms = self.knowledge.grasp_xforms(object);
#try all of the grasps and stop if one is successful
for grasp_t in grasp_transforms:
#get the world point goal which is offset from the grasp_t
rot_xform = se3.mul(grasp_t,se3.inv(left_gripper_center_xform))
#rot = so3.rotation([0,0,1],math.pi);
#rot_t = [rot,[0,0,0]];
world_goal_xyz = rot_xform[1];
cnt = 0;
while(cnt < 100):
#pick a random starting config for the left link and try to solve the ik problem
config = self.robot.getConfig()
for ind in self.left_arm_indices:
config[ind] = random.uniform(0,3);
self.robot.setConfig(config);
goal = ik.objective(self.left_gripper_link,R=se3.mul(grasp_t,se3.identity())[0],t=world_goal_xyz)
if ik.solve(goal):
self.active_limb = 'Left'
self.config = self.robot.getConfig()
return True
cnt = cnt + 1
return False
def emulate(self,sim):
"""Given a Simulator instance, emulates the sensor.
Result is a CameraColorDetectorOutput structure."""
global objectColors
xscale = math.tan(math.radians(self.fov*0.5))*self.w/2
blobs = []
for i in range(sim.world.numRigidObjects()):
o = sim.world.rigidObject(i)
body = sim.getBody(o)
Tb = body.getTransform()
plocal = se3.apply(se3.inv(self.Tsensor),Tb[1])
x,y,z = plocal
if z < self.dmin or z > self.dmax:
continue
c = objectColors[i%len(objectColors)]
o.geometry().setCurrentTransform(*se3.identity())
bmin,bmax = o.geometry().getBB()
err = self.pixelError
dscale = xscale/z
xim = dscale * x + self.w/2
yim = dscale * y + self.h/2
xmin = int(xim - dscale*(bmax[0]-bmin[0])*0.5 + random.uniform(-err,err))
ymin = int(yim - dscale*(bmax[1]-bmin[1])*0.5 + random.uniform(-err,err))
xmax = int(xim + dscale*(bmax[0]-bmin[0])*0.5 + random.uniform(-err,err))
ymax = int(yim + dscale*(bmax[1]-bmin[1])*0.5 + random.uniform(-err,err))
if xmin < 0: xmin=0
if ymin < 0: ymin=0
if xmax >= self.w: xmax=self.w-1
if ymax >= self.h: ymax=self.h-1
if ymax <= ymin: continue
if xmax <= xmin: continue
#print "Actual x,y,z",x,y,z
#print "blob color",c[0:3],"dimensions",xmin,xmax,"x",ymin,ymax
blob = CameraBlob(c[0:3],0.5*(xmin+xmax),0.5*(ymin+ymax),xmax-xmin,ymax-ymin)
blobs.append(blob)
return CameraColorDetectorOutput(blobs)
def emulate(self,sim):
"""Given a Simulator instance, emulates the sensor.
Result is a CameraColorDetectorOutput structure."""
global objectColors
xscale = math.tan(math.radians(self.fov*0.5))*self.w/2
blobs = []
for i in range(sim.world.numRigidObjects()):
o = sim.world.rigidObject(i)
body = sim.getBody(o)
Tb = body.getTransform()
plocal = se3.apply(se3.inv(self.Tsensor),Tb[1])
x,y,z = plocal
if z < self.dmin or z > self.dmax:
continue
c = objectColors[i%len(objectColors)]
o.geometry().setCurrentTransform(*se3.identity())
bmin,bmax = o.geometry().getBB()
err = self.pixelError
dscale = xscale/z
xim = dscale * x + self.w/2
yim = dscale * y + self.h/2
xmin = int(xim - dscale*(bmax[0]-bmin[0])*0.5 + random.uniform(-err,err))
ymin = int(yim - dscale*(bmax[1]-bmin[1])*0.5 + random.uniform(-err,err))
xmax = int(xim + dscale*(bmax[0]-bmin[0])*0.5 + random.uniform(-err,err))
ymax = int(yim + dscale*(bmax[1]-bmin[1])*0.5 + random.uniform(-err,err))
if xmin < 0: xmin=0
if ymin < 0: ymin=0
if xmax >= self.w: xmax=self.w-1
if ymax >= self.h: ymax=self.h-1
if ymax <= ymin: continue
if xmax <= xmin: continue
#print "Actual x,y,z",x,y,z
#print "blob color",c[0:3],"dimensions",xmin,xmax,"x",ymin,ymax
blob = CameraBlob(c[0:3],0.5*(xmin+xmax),0.5*(ymin+ymax),xmax-xmin,ymax-ymin)
blobs.append(blob)
return CameraColorDetectorOutput(blobs)
def __init__(self,zero=se3.identity(),prior=0.0):
self.average = zero
self.count = prior
def localizeShelf(self):
logger.info('localizing shelf')
if fake_wait(10, 0.95):
return se3.identity()
else:
return None
def display(self):
# you may run auxiliary openGL calls, if you wish to visually debug
# self.world.robot(0).setConfig(self.controller.config)
self.world.drawGL()
# show bin boxes
if self.draw_bins:
glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE,[1,1,0,1])
for b in apc.bin_bounds.values():
draw_oriented_box(ground_truth_shelf_xform,b[0],b[1])
for b in apc.bin_names:
c = knowledge.bin_front_center(b)
if c:
glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE,[1,1,0.5,1])
r = 0.01
gldraw.box([c[0]-r,c[1]-r,c[2]-r],[c[0]+r,c[1]+r,c[2]+r])
c = knowledge.bin_vantage_point(b)
if c:
glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE,[0.5,1,0.5,1])
r = 0.01
gldraw.box([c[0]-r,c[1]-r,c[2]-r],[c[0]+r,c[1]+r,c[2]+r])
# show object state
for i in ground_truth_items:
if i.xform == None:
continue
if i.bin_name == 'order_bin':
# draw in wireframe
glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE,[1,0.5,0,1])
draw_oriented_box(i.xform,i.info.bmin,i.info.bmax)
continue
# if perceived bin has an item, draw it in solid color
if knowledge.bin_contents[i.bin_name]!=None and i in knowledge.bin_contents[i.bin_name]:
glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE,[1,0.5,0,1])
draw_oriented_box(i.xform,i.info.bmin,i.info.bmax)
else:
# otherwise, draw in wireframe
glDisable(GL_LIGHTING)
glColor3f(1,0.5,0)
draw_oriented_wire_box(i.xform,i.info.bmin,i.info.bmax)
glEnable(GL_LIGHTING)
if self.draw_grasps:
# draw grasps, if available
g = knowledge.grasp_xforms(i)
if g:
for xform in g:
gldraw.xform_widget(xform,0.02,0.002)
# Show gripper and camera frames
if self.draw_gripper_and_camera:
left_camera_link = self.world.robot(0).link(left_camera_link_name)
right_camera_link = self.world.robot(0).link(right_camera_link_name)
left_gripper_link = self.world.robot(0).link(left_gripper_link_name)
right_gripper_link = self.world.robot(0).link(right_gripper_link_name)
gldraw.xform_widget(left_camera_link.getTransform(),0.1,0.01)
gldraw.xform_widget(right_camera_link.getTransform(),0.1,0.01)
gldraw.xform_widget(se3.mul(left_gripper_link.getTransform(),left_gripper_center_xform),0.05,0.005, lighting=False, fancy=True)
gldraw.xform_widget(se3.mul(right_gripper_link.getTransform(),right_gripper_center_xform),0.05,0.005, lighting=False, fancy=True)
# Show world frame and shelf frame
gldraw.xform_widget(ground_truth_shelf_xform, 0.1, 0.015, lighting=False, fancy=True)
gldraw.xform_widget(se3.identity(), 0.2, 0.037, lighting=False, fancy=True)
# Draw order box
glDisable(GL_LIGHTING)
glColor3f(1,0,0)
draw_oriented_wire_box(order_bin_xform,order_bin_bounds[0],order_bin_bounds[1])
glEnable(GL_LIGHTING)
return
def __init__(self):
self.Tsensor = se3.identity()
self.fov = 90
self.w,self.h = 320,240
self.dmin,self.dmax = 0.01,5
self.pixelError = 0.5
def __init__(self, address, port=10000, xform=None):
self.raw = RemoteRawCamera(address, port)
self.xform = xform or se3.identity()
def __init__(self):
self.bin_contents = dict((n,None) for n in apc.bin_names)
self.order_bin_contents = []
self.shelf_xform = se3.identity()
#you may want to implement this. Returns a list of world transformations
#for the grasps defined for the given object (an ItemInBin instance).
#The visualizer will draw these transforms if 'draw_grasps' is turned to True.
l = [x.grasp_xform for x in object.info.grasps]
r = [se3.mul(object.xform,x) for x in l]
#if len(r) > 11:
# return [r[11]]
#else:
# return None
return r
#a list of actual items -- this is only used for the fake perception module, and your
#code should not use these items directly
ground_truth_items = []
ground_truth_shelf_xform = se3.identity()
def init_ground_truth():
global ground_truth_items
ground_truth_items = [apc.ItemInBin(apc.tall_item,'bin_B'),
apc.ItemInBin(apc.small_item,'bin_D'),
apc.ItemInBin(apc.med_item,'bin_H')]
ground_truth_items[0].set_in_bin_xform(ground_truth_shelf_xform,0.2,0.2,0.0)
ground_truth_items[1].set_in_bin_xform(ground_truth_shelf_xform,0.5,0.1,math.pi/4)
ground_truth_items[2].set_in_bin_xform(ground_truth_shelf_xform,0.6,0.4,math.pi/2)
def run_perception_on_bin(knowledge,bin_name):
"""This is a fake perception module that simply reveals all the items
the given bin."""
global ground_truth_items
if knowledge.bin_contents[bin_name]==None:
#not sensed yet
print "With no arguments, shows some 3D trajectories"
#add a point to the visualizer and animate it
point = coordinates.addPoint("point")
visualization.add("point",point)
traj = trajectory.Trajectory()
for i in range(10):
traj.times.append(i)
traj.milestones.append([random.uniform(-1,1),random.uniform(-1,1),random.uniform(-1,1)])
traj2 = trajectory.HermiteTrajectory()
traj2.makeSpline(traj)
visualization.animate("point",traj2)
#add a transform to the visualizer and animate it
xform = visualization.add("xform",se3.identity())
traj3 = trajectory.SE3Trajectory()
for i in range(10):
traj3.times.append(i)
rrot = random_rotation()
rpoint = [random.uniform(-1,1),random.uniform(-1,1),random.uniform(-1,1)]
traj3.milestones.append(rrot+rpoint)
visualization.animate("xform",traj3)
else:
#creates a world and loads all the items on the command line
world = WorldModel()
for fn in sys.argv[1:]:
res = world.readFile(fn)
if not res:
raise RuntimeError("Unable to load model "+fn)
def display(self):
#draw the world
self.sim.updateWorld()
self.simworld.drawGL()
#draw commanded configurations
glEnable(GL_BLEND)
glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA)
glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE,[0,1,0,0.5])
# only 1 robot in this case, but still use for-loop for generality
for i in xrange(self.simworld.numRobots()):
r = self.simworld.robot(i)
q = self.low_level_controller.getCommandedConfig()
r.setConfig(q)
r.drawGL(False)
glDisable(GL_BLEND)
self.drawContactForces()
# self.drawRoadmap()
# add commands to queue if automatically restarted
global automatic
if automatic >= 1:
automatic += 1
if automatic == 10:
self.command_queue.put('l')
self.command_queue.put('x')
self.command_queue.put('t')
automatic = -1
# print self.sim.inContact(i,j)
# print self.simworld.getName(i),"-",self.simworld.getName(j), f
# t = self.sim.mean
# printf("%s - %s: force %g %g %g\n",world.GetName(i).c_str(),world.GetName(j)).c_str(),f.x,f.y,f.z,t.x,t.y,t.z);
# print contact.simContactMap(self.sim)
# qSim = self.simworld.robot(0).getConfig()
# qReal = self.low_level_controller.getSensedConfig()
# diff = vectorops.distance(qSim, qReal)
# print diff
# print self.sim.getActualTorques(0)[1]
# print self.sim.getContactForces(1,2)
# print self.simworld.robot(0).getID()
# print self.simworld.rigidObject(0).getID()
# self.sim.enableContactFeedbackAll()
# print max(abs(self.sim.getJointForces(self.simworld.robot(0).link(9))[3:6]))
# print max(abs(self.sim.getJointForces(self.simworld.robot(0).link(14))[3:6]))
# print max(abs(self.sim.getJointForces(self.simworld.robot(0).link(18))[3:6]))
# print self.sim.getJointForces(self.simworld.robot(0).link(9))
if self.showFrames:
# world frame
gldraw.xform_widget(se3.identity(), 0.15, 0.005, lighting=True, fancy=True)
# robot frame
gldraw.xform_widget(self.simworld.robot(0).link(6).getTransform(), 0.15, 0.005, lighting=True, fancy=True)
# object frame
gldraw.xform_widget(self.simworld.rigidObject(0).getTransform(), 0.15, 0.005, lighting = True, fancy = True)
return
def __init__(self,zero=se3.identity(),prior=0.0):
self.average = zero
self.count = prior
def calibrate_xform_camera(camera_link_transforms,
marker_link_transforms,
marker_observations,
marker_ids,
observation_relative_errors=None,
camera_initial_guess=None,
marker_initial_guess=None,
regularizationFactor=0,
maxIters=100,
tolerance=1e-7):
"""Single camera calibration function for a camera and markers on some
set of rigid bodies.
Given body transforms and a list of estimated calibration marker observations
in the camera frame, estimates both the camera transform relative to the
camera link as well as the marker transforms relative to their links.
M: is the set of m markers. By default there is at most one marker per link.
Markers can either be point markers (e.g., a mocap ball), or transform
markers (e.g., an AR tag or checkerboard pattern).
O: is the set of n observations, consisting of a reading (Tc_i,Tm_i,o_i,l_i)
where Tc_i is the camera link's transform, Tm_i is the marker link's
transform, o_i is the reading which consists of either a point or transform
estimate in the camera frame, and l_i is the ID of the marker (by default,
just its link)
Output: a tuple (err,Tc,marker_dict) where err is the norm of the
reconstruction residual, Tc is the estimated camera transform relative to the
camera's link, and marker_dict is a dict mapping each marker id to its
estimated position or transform on the marker's link.
Arguments:
- camera_link: an integer index or a RobotModelLink instance on which
the camera lies.
- calibration_configs: a list of the RobotModel configurations q_1,...,q_n
that generated the marker_observations list.
- marker_observations: a list of estimated positions or transformations
of calibration markers o_1,...,o_n, given in the camera's reference
frame (z forward, x right, y down).
If o_i is a 3-vector, the marker is considered to be a point marker.
If a se3 element (R,t) is given, the marker is considered to be
a transform marker. You may not mix point and transform observations
for a single marker ID.
- marker_ids: a list of marker ID #'s l_1,...,l_n corresponding to
each observation, e.g., the link index on which each marker lies.
- observation_relative_errors: if you have an idea of the magnitude of
each observation error, it can be placed into this list. Must be
a list of n floats, 3-lists (point markers), or 6-lists (transform
markers). By default, errors will be set proportionally to the
observed distance between the camera and marker.
- camera_initial_guess: if not None, an initial guess for the camera transform
- marker_initial_guess: if not None, a dictionary containing initial guesses
for the marker transforms
- regularizationFactor: if nonzero, the optimization penalizes deviation
of the estimated camera transform and marker transforms from zero
proportionally to this factor.
- maxIters: maximum number of iterations for optimization.
- tolerance: optimization convergence tolerance. Stops when the change of
estimates falls below this threshold
"""
if len(camera_link_transforms) != len(marker_ids):
raise ValueError("Must provide the same number of marker IDs as camera transforms")
if len(marker_link_transforms) != len(marker_ids):
raise ValueError("Must provide the same number of marker IDs as marker transforms")
if len(marker_observations) != len(marker_ids):
raise ValueError("Must provide the same number of marker observations as marker transforms")
#get all unique marker ids
marker_id_list = list(set(marker_ids))
#detect marker types
marker_types = dict((v,None) for v in marker_id_list)
for i,(obs,id) in enumerate(zip(marker_observations,marker_ids)):
if len(obs)==3:
if marker_types[id] == 't':
raise ValueError("Provided both point and transform observations for observation #%d, id %s\n"%(i,str(id)))
marker_types[id] = 'p'
elif len(obs)==2 and len(obs[0])==9 and len(obs[1])==3:
if marker_types[id] == 'p':
raise ValueError("Provided both point and transform observations for observation #%d, id %s\n"%(i,str(id)))
marker_types[id] = 't'
else:
raise ValueError("Invalid observation for observation #%d, id %s\n"%(i,str(id)))
n = len(marker_observations)
m = len(marker_id_list)
#get all the observation weights
observation_weights = []
if observation_relative_errors is None:
#default weights: proportional to distance
for obs in marker_observations:
if len(obs) == 3:
observation_weights.append(1.0/vectorops.norm(obs))
else:
observation_weights.append(1.0/vectorops.norm(obs[1]))
observation_weights = [1.0]*len(observation_weights)
else:
if len(observation_relative_errors) != n:
raise ValueError("Invalid length of observation errors")
for err in observation_relative_errors:
if hasattr(err,'__iter__'):
observation_weights.append([1.0/v for v in err])
else:
observation_weights.append(1.0/err)
#initial guesses
if camera_initial_guess == None:
camera_initial_guess = se3.identity()
if any(v == 't' for v in marker_types.itervalues()):
#estimate camera rotation from point estimates because rotations are more prone to initialization failures
point_observations = []
marker_point_rel = []
for i,obs in enumerate(marker_observations):
if len(obs)==2:
point_observations.append(obs[1])
else:
point_observations.append(obs)
marker_point_rel.append(se3.mul(se3.inv(camera_link_transforms[i]),marker_link_transforms[i])[1])
camera_initial_guess = (point_fit_rotation_3d(point_observations,marker_point_rel),[0.0]*3)
print "Estimated camera rotation from points:",camera_initial_guess
if marker_initial_guess == None:
marker_initial_guess = dict((l,(se3.identity() if marker_types[l]=='t' else [0.0]*3)) for l in marker_id_list)
else:
marker_initial_guess = marker_initial_guess.copy()
for l in marker_id_list:
if l not in marker_initial_guess:
marker_initial_guess[l] = (se3.identity() if marker_types[l]=='t' else [0.0]*3)
camera_transform = camera_initial_guess
marker_transforms = marker_initial_guess.copy()
if DO_VISUALIZATION:
rgroup = coordinates.addGroup("calibration")
rgroup.addFrame("camera link",worldCoordinates=camera_link_transforms[-1])
rgroup.addFrame("marker link",worldCoordinates=marker_link_transforms[-1])
rgroup.addFrame("camera estimate",parent="camera link",relativeCoordinates=camera_transform)
rgroup.addFrame("marker estimate",parent="marker link",relativeCoordinates=marker_transforms.values()[0])
for i,obs in enumerate(marker_observations):
rgroup.addFrame("obs"+str(i)+" estimate",parent="camera estimate",relativeCoordinates=obs)
vis.add("coordinates",rgroup)
vis.dialog()
point_observations_only = all(marker_types[marker] == 'p' for marker in marker_id_list)
xform_observations_only = all(marker_types[marker] == 't' for marker in marker_id_list)
if not point_observations_only and not xform_observations_only:
raise NotImplementedError("Can't calibrate camera from mixed point/transform markers yet")
for iters in range(maxIters):
#attempt to minimize the error on the following over all observations i
#camera_link_transform(q_i)*camera_transform*observation_i = marker_link_transform(l_i,q_i)*marker_transform(l_i)
#first, we'll assume the camera transform is fixed and then optimize the marker transforms.
#then, we'll assume the marker transforms are fixed and then optimize the camera transform.
#finally we'll check the error to detect convergence
#1. Estimate marker transforms from current camera transform
new_marker_transforms = dict((l,(TransformStats(zero=marker_initial_guess[l],prior=regularizationFactor) if marker_types[l]=='t' else VectorStats(value,zero=[0.0]*3,prior=RegularizationFactor))) for l in marker_id_list)
for i in xrange(n):
marker = marker_ids[i]
Tclink = camera_link_transforms[i]
Tmlink = marker_link_transforms[i]
obs = marker_observations[i]
Trel = se3.mul(se3.inv(Tmlink),se3.mul(Tclink,camera_transform))
if marker_types[marker] == 't':
estimate = se3.mul(Trel,obs)
else:
estimate = se3.apply(Trel,obs)
new_marker_transforms[marker].add(estimate,observation_weights[i])
print "ITERATION",iters
#print " ESTIMATED MARKER TRANSFORMS:",dict((k,v.average) for (k,v) in new_marker_transforms.iteritems())
#2. Estimate camera transform from current marker transforms
new_camera_transform = TransformStats(zero=camera_initial_guess,prior=regularizationFactor)
if point_observations_only:
#TODO: weighted point fitting
relative_points = []
for i in xrange(n):
marker = marker_ids[i]
Tclink = camera_link_transforms[i]
Tmlink = marker_link_transforms[i]
obs = marker_observations[i]
pRel = se3.apply(se3.inv(Tclink),se3.apply(Tmlink,new_marker_transforms[marker].average))
relative_points.append(pRel)
new_camera_transform.add(point_fit_xform_3d(marker_observations,relative_points),sum(observation_weights))
else:
for i in xrange(n):
marker = marker_ids[i]
Tclink = camera_link_transforms[i]
Tmlink = marker_link_transforms[i]
obs = marker_observations[i]
Trel = se3.mul(se3.inv(Tclink),se3.mul(Tmlink,new_marker_transforms[marker].average))
estimate = se3.mul(Trel,se3.inv(obs))
new_camera_transform.add(estimate,observation_weights[i])
#print " ESTIMATED CAMERA TRANSFORMS:",new_camera_transform.average
#3. compute difference between last and current estimates
diff = 0.0
diff += vectorops.normSquared(se3.error(camera_transform,new_camera_transform.average))
for marker in marker_id_list:
if marker_types[marker]=='t':
diff += vectorops.normSquared(se3.error(marker_transforms[marker],new_marker_transforms[marker].average))
else:
diff += vectorops.distanceSquared(marker_transforms[marker],new_marker_transforms[marker].average)
camera_transform = new_camera_transform.average
for marker in marker_id_list:
marker_transforms[marker] = new_marker_transforms[marker].average
if math.sqrt(diff) < tolerance:
#converged!
print "Converged with diff %g on iteration %d"%(math.sqrt(diff),iters)
break
print " ESTIMATE CHANGE:",math.sqrt(diff)
error = 0.0
for i in xrange(n):
marker = marker_ids[i]
Tclink = camera_link_transforms[i]
Tmlink = marker_link_transforms[i]
obs = marker_observations[i]
Tc = se3.mul(Tclink,camera_transform)
if marker_types[marker] == 't':
Tm = se3.mul(Tmlink,marker_transforms[marker])
error += vectorops.normSquared(se3.error(se3.mul(Tc,obs),Tm))
else:
Tm = se3.apply(Tmlink,marker_transforms[marker])
error += vectorops.distanceSquared(se3.apply(Tc,obs),Tm)
print " OBSERVATION ERROR:",math.sqrt(error)
#raw_input()
if DO_VISUALIZATION:
rgroup.setFrameCoordinates("camera estimate",camera_transform)
rgroup.setFrameCoordinates("marker estimate",marker_transforms.values()[0])
for i,obs in enumerate(marker_observations):
rgroup.setFrameCoordinates("obs"+str(i)+" estimate",obs)
vis.add("coordinates",rgroup)
vis.dialog()
if math.sqrt(diff) >= tolerance:
print "Max iters reached"
error = 0.0
for i in xrange(n):
marker = marker_ids[i]
Tclink = camera_link_transforms[i]
Tmlink = marker_link_transforms[i]
obs = marker_observations[i]
Tc = se3.mul(Tclink,camera_transform)
if marker_types[marker] == 't':
Tm = se3.mul(Tmlink,marker_transforms[marker])
error += vectorops.normSquared(se3.error(se3.mul(Tc,obs),Tm))
else:
Tm = se3.apply(Tmlink,marker_transforms[marker])
error += vectorops.distanceSquared(se3.apply(Tc,obs),Tm)
return (math.sqrt(error),camera_transform,marker_transforms)
setattr(kb, k, data)
#test: visualize result
#for v in kb.vantage_point_xforms:
# (save,result) = resource.edit(v,kb.vantage_point_xforms[v],'RigidTransform',description="vantage point for bin "+v,world=world)
r = PerceptionInterface(kb)
kb.shelf_xform = r.localizeShelf()
kb.order_bin_xform = r.localizeOrderBin()
#kb.object_xforms['crayola_64_ct'] = (so3.rotation([0,0,1], 3.1415/4+3.1415), (1.0, 0.15, 0.70))
# kb.object_xforms['kong_sitting_frog_dog_toy'] = se3.identity()
# kb.object_clouds['kong_sitting_frog_dog_toy'] = pcd.read(open('/tmp/kong_sitting_frog_dog_toy_bin_C.pcd'))[1]
kb.object_xforms['safety_works_safety_glasses'] = se3.identity()
kb.object_clouds['safety_works_safety_glasses'] = pcd.read(open('/tmp/safety_works_safety_glasses_bin_G.pcd'))[1]
kb.object_xforms['stanley_66_052'] = se3.identity()
kb.object_clouds['stanley_66_052'] = pcd.read(open('/tmp/stanley_66_052_bin_F.pcd'))[1]
p = PlanningInterface(kb)
p.planMoveToInitialPose()
kb.target_bin = 'bin_F'
plan = p.planGraspObjectInBin('bin_F','stanley_66_052')
def localizeOrderBin(self):
logger.info('localizing order bin')
if fake_wait(10, 0.95):
return se3.identity()
else:
return None
def localizeSpecificObject(self, bin, object):
logger.info('localizing {} in {}'.format(bin, object))
if fake_wait(10, 0.8):
return (se3.identity(), pcd.read(open('/tmp/expo_dry_erase_board_eraser_bin_D.pcd'))[1], dict(zip(self.knowledge_base.bin_contents[bin], [1.0 / len(self.knowledge_base.bin_contents[bin])] * len(self.knowledge_base.bin_contents))))
else:
return None
visualization.add("point", point)
traj = trajectory.Trajectory()
for i in range(10):
traj.times.append(i)
traj.milestones.append([
random.uniform(-1, 1),
random.uniform(-1, 1),
random.uniform(-1, 1)
])
traj2 = trajectory.HermiteTrajectory()
traj2.makeSpline(traj)
visualization.animate("point", traj2)
#add a transform to the visualizer and animate it
xform = visualization.add("xform", se3.identity())
traj3 = trajectory.SE3Trajectory()
for i in range(10):
traj3.times.append(i)
rrot = random_rotation()
rpoint = [
random.uniform(-1, 1),
random.uniform(-1, 1),
random.uniform(-1, 1)
]
traj3.milestones.append(rrot + rpoint)
visualization.animate("xform", traj3)
else:
#creates a world and loads all the items on the command line
world = WorldModel()
def display(self):
#you may run auxiliary openGL calls, if you wish to visually debug
#draw the world
self.sim.updateWorld()
self.simworld.drawGL()
#if you're doing question 1, this will draw the shelf and floor
if self.simworld.numTerrains()==0:
for i in range(self.planworld.numTerrains()):
self.planworld.terrain(i).drawGL()
#draw commanded configurations
glEnable(GL_BLEND)
glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA)
glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE,[0,1,0,0.5])
# only 1 robot in this case, but still use for-loop for generality
for i in xrange(self.simworld.numRobots()):
r = self.simworld.robot(i)
#q = self.sim.controller(i).getCommandedConfig()
q = self.low_level_controller.getCommandedConfig()
r.setConfig(q)
r.drawGL(False)
glDisable(GL_BLEND)
#show bin boxes
if self.draw_bins:
glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE,[1,1,0,1])
for b in apc.bin_bounds.values():
draw_oriented_box(self.picking_controller.knowledge.shelf_xform,b[0],b[1])
for b in apc.bin_names:
c = self.picking_controller.knowledge.bin_front_center(b)
if c:
glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE,[1,1,0.5,1])
r = 0.01
gldraw.box([c[0]-r,c[1]-r,c[2]-r],[c[0]+r,c[1]+r,c[2]+r])
c = self.picking_controller.knowledge.bin_vantage_point(b)
if c:
glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE,[0.5,1,0.5,1])
r = 0.01
gldraw.box([c[0]-r,c[1]-r,c[2]-r],[c[0]+r,c[1]+r,c[2]+r])
#show object state
for i in ground_truth_items:
if i.xform == None:
continue
if i.bin_name == 'order_bin':
continue
#if perceived, draw in solid color
if self.picking_controller.knowledge.bin_contents[i.bin_name]!=None and i in self.picking_controller.knowledge.bin_contents[i.bin_name]:
glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE,[1,0.5,0,1])
draw_oriented_box(i.xform,i.info.bmin,i.info.bmax)
else:
#otherwise, draw in wireframe
glDisable(GL_LIGHTING)
glColor3f(1,0.5,0)
draw_oriented_wire_box(i.xform,i.info.bmin,i.info.bmax)
glEnable(GL_LIGHTING)
if self.draw_grasps:
#draw grasps, if available
g = self.picking_controller.knowledge.grasp_xforms(i)
if g:
for grasp,xform in g:
gldraw.xform_widget(xform,0.05,0.005,fancy=False)
# Draws the object held on gripper
obj,limb,grasp = self.picking_controller.held_object,self.picking_controller.active_limb,self.picking_controller.active_grasp
if obj != None:
if limb == 'left':
gripper_xform = self.simworld.robot(0).link(left_gripper_link_name).getTransform()
else:
gripper_xform = self.simworld.robot(0).link(right_gripper_link_name).getTransform()
objxform = se3.mul(gripper_xform,se3.mul(left_gripper_center_xform,se3.inv(grasp.grasp_xform)))
glDisable(GL_LIGHTING)
glColor3f(1,1,1)
draw_oriented_wire_box(objxform,obj.info.bmin,obj.info.bmax)
glEnable(GL_LIGHTING)
#show gripper and camera frames
if self.draw_gripper_and_camera:
left_camera_link = self.simworld.robot(0).link(left_camera_link_name)
right_camera_link = self.simworld.robot(0).link(right_camera_link_name)
left_gripper_link = self.simworld.robot(0).link(left_gripper_link_name)
right_gripper_link = self.simworld.robot(0).link(right_gripper_link_name)
gldraw.xform_widget(left_camera_link.getTransform(),0.1,0.01,fancy=False)
gldraw.xform_widget(right_camera_link.getTransform(),0.1,0.01,fancy=False)
gldraw.xform_widget(se3.mul(left_gripper_link.getTransform(),left_gripper_center_xform),0.05,0.005,fancy=False)
gldraw.xform_widget(se3.mul(right_gripper_link.getTransform(),right_gripper_center_xform),0.05,0.005,fancy=False)
# Show world frame and shelf frame
gldraw.xform_widget(ground_truth_shelf_xform, 0.1, 0.015, lighting=False, fancy=True)
gldraw.xform_widget(se3.identity(), 0.2, 0.037, lighting=False, fancy=True)
#draw order box
glDisable(GL_LIGHTING)
glColor3f(1,0,0)
draw_oriented_wire_box(order_bin_xform,order_bin_bounds[0],order_bin_bounds[1])
glEnable(GL_LIGHTING)
return
def __init__(self, address, port=10000, xform=None):
self.raw = RemoteRawCamera(address, port)
self.xform = xform or se3.identity()
def calibrate_xform_camera(camera_link_transforms,
marker_link_transforms,
marker_observations,
marker_ids,
observation_relative_errors=None,
camera_initial_guess=None,
marker_initial_guess=None,
regularizationFactor=0,
maxIters=100,
tolerance=1e-7):
"""Single camera calibration function for a camera and markers on some
set of rigid bodies.
Given body transforms and a list of estimated calibration marker observations
in the camera frame, estimates both the camera transform relative to the
camera link as well as the marker transforms relative to their links.
M: is the set of m markers. By default there is at most one marker per link.
Markers can either be point markers (e.g., a mocap ball), or transform
markers (e.g., an AR tag or checkerboard pattern).
O: is the set of n observations, consisting of a reading (Tc_i,Tm_i,o_i,l_i)
where Tc_i is the camera link's transform, Tm_i is the marker link's
transform, o_i is the reading which consists of either a point or transform
estimate in the camera frame, and l_i is the ID of the marker (by default,
just its link)
Output: a tuple (err,Tc,marker_dict) where err is the norm of the
reconstruction residual, Tc is the estimated camera transform relative to the
camera's link, and marker_dict is a dict mapping each marker id to its
estimated position or transform on the marker's link.
Arguments:
- camera_link: an integer index or a RobotModelLink instance on which
the camera lies.
- calibration_configs: a list of the RobotModel configurations q_1,...,q_n
that generated the marker_observations list.
- marker_observations: a list of estimated positions or transformations
of calibration markers o_1,...,o_n, given in the camera's reference
frame (z forward, x right, y down).
If o_i is a 3-vector, the marker is considered to be a point marker.
If a se3 element (R,t) is given, the marker is considered to be
a transform marker. You may not mix point and transform observations
for a single marker ID.
- marker_ids: a list of marker ID #'s l_1,...,l_n corresponding to
each observation, e.g., the link index on which each marker lies.
- observation_relative_errors: if you have an idea of the magnitude of
each observation error, it can be placed into this list. Must be
a list of n floats, 3-lists (point markers), or 6-lists (transform
markers). By default, errors will be set proportionally to the
observed distance between the camera and marker.
- camera_initial_guess: if not None, an initial guess for the camera transform
- marker_initial_guess: if not None, a dictionary containing initial guesses
for the marker transforms
- regularizationFactor: if nonzero, the optimization penalizes deviation
of the estimated camera transform and marker transforms from zero
proportionally to this factor.
- maxIters: maximum number of iterations for optimization.
- tolerance: optimization convergence tolerance. Stops when the change of
estimates falls below this threshold
"""
if len(camera_link_transforms) != len(marker_ids):
raise ValueError("Must provide the same number of marker IDs as camera transforms")
if len(marker_link_transforms) != len(marker_ids):
raise ValueError("Must provide the same number of marker IDs as marker transforms")
if len(marker_observations) != len(marker_ids):
raise ValueError("Must provide the same number of marker observations as marker transforms")
#get all unique marker ids
marker_id_list = list(set(marker_ids))
#detect marker types
marker_types = dict((v,None) for v in marker_id_list)
for i,(obs,id) in enumerate(zip(marker_observations,marker_ids)):
if len(obs)==3:
if marker_types[id] == 't':
raise ValueError("Provided both point and transform observations for observation #%d, id %s\n"%(i,str(id)))
marker_types[id] = 'p'
elif len(obs)==2 and len(obs[0])==9 and len(obs[1])==3:
if marker_types[id] == 'p':
raise ValueError("Provided both point and transform observations for observation #%d, id %s\n"%(i,str(id)))
marker_types[id] = 't'
else:
raise ValueError("Invalid observation for observation #%d, id %s\n"%(i,str(id)))
n = len(marker_observations)
m = len(marker_id_list)
#get all the observation weights
observation_weights = []
if observation_relative_errors is None:
#default weights: proportional to distance
for obs in marker_observations:
if len(obs) == 3:
observation_weights.append(1.0/vectorops.norm(obs))
else:
observation_weights.append(1.0/vectorops.norm(obs[1]))
observation_weights = [1.0]*len(observation_weights)
else:
if len(observation_relative_errors) != n:
raise ValueError("Invalid length of observation errors")
for err in observation_relative_errors:
if hasattr(err,'__iter__'):
observation_weights.append([1.0/v for v in err])
else:
observation_weights.append(1.0/err)
#initial guesses
if camera_initial_guess == None:
camera_initial_guess = se3.identity()
if any(v == 't' for v in marker_types.itervalues()):
#estimate camera rotation from point estimates because rotations are more prone to initialization failures
point_observations = []
marker_point_rel = []
for i,obs in enumerate(marker_observations):
if len(obs)==2:
point_observations.append(obs[1])
else:
point_observations.append(obs)
marker_point_rel.append(se3.mul(se3.inv(camera_link_transforms[i]),marker_link_transforms[i])[1])
camera_initial_guess = (point_fit_rotation_3d(point_observations,marker_point_rel),[0.0]*3)
print "Estimated camera rotation from points:",camera_initial_guess
if marker_initial_guess == None:
marker_initial_guess = dict((l,(se3.identity() if marker_types[l]=='t' else [0.0]*3)) for l in marker_id_list)
else:
marker_initial_guess = marker_initial_guess.copy()
for l in marker_id_list:
if l not in marker_initial_guess:
marker_initial_guess[l] = (se3.identity() if marker_types[l]=='t' else [0.0]*3)
camera_transform = camera_initial_guess
marker_transforms = marker_initial_guess.copy()
if DO_VISUALIZATION:
rgroup = coordinates.addGroup("calibration")
rgroup.addFrame("camera link",worldCoordinates=camera_link_transforms[-1])
rgroup.addFrame("marker link",worldCoordinates=marker_link_transforms[-1])
rgroup.addFrame("camera estimate",parent="camera link",relativeCoordinates=camera_transform)
rgroup.addFrame("marker estimate",parent="marker link",relativeCoordinates=marker_transforms.values()[0])
for i,obs in enumerate(marker_observations):
rgroup.addFrame("obs"+str(i)+" estimate",parent="camera estimate",relativeCoordinates=obs)
visualization.add("coordinates",rgroup)
visualization.dialog()
point_observations_only = all(marker_types[marker] == 'p' for marker in marker_id_list)
xform_observations_only = all(marker_types[marker] == 't' for marker in marker_id_list)
if not point_observations_only and not xform_observations_only:
raise NotImplementedError("Can't calibrate camera from mixed point/transform markers yet")
for iters in range(maxIters):
#attempt to minimize the error on the following over all observations i
#camera_link_transform(q_i)*camera_transform*observation_i = marker_link_transform(l_i,q_i)*marker_transform(l_i)
#first, we'll assume the camera transform is fixed and then optimize the marker transforms.
#then, we'll assume the marker transforms are fixed and then optimize the camera transform.
#finally we'll check the error to detect convergence
#1. Estimate marker transforms from current camera transform
new_marker_transforms = dict((l,(TransformStats(zero=marker_initial_guess[l],prior=regularizationFactor) if marker_types[l]=='t' else VectorStats(value,zero=[0.0]*3,prior=RegularizationFactor))) for l in marker_id_list)
for i in xrange(n):
marker = marker_ids[i]
Tclink = camera_link_transforms[i]
Tmlink = marker_link_transforms[i]
obs = marker_observations[i]
Trel = se3.mul(se3.inv(Tmlink),se3.mul(Tclink,camera_transform))
if marker_types[marker] == 't':
estimate = se3.mul(Trel,obs)
else:
estimate = se3.apply(Trel,obs)
new_marker_transforms[marker].add(estimate,observation_weights[i])
print "ITERATION",iters
#print " ESTIMATED MARKER TRANSFORMS:",dict((k,v.average) for (k,v) in new_marker_transforms.iteritems())
#2. Estimate camera transform from current marker transforms
new_camera_transform = TransformStats(zero=camera_initial_guess,prior=regularizationFactor)
if point_observations_only:
#TODO: weighted point fitting
relative_points = []
for i in xrange(n):
marker = marker_ids[i]
Tclink = camera_link_transforms[i]
Tmlink = marker_link_transforms[i]
obs = marker_observations[i]
pRel = se3.apply(se3.inv(Tclink),se3.apply(Tmlink,new_marker_transforms[marker].average))
relative_points.append(pRel)
new_camera_transform.add(point_fit_xform_3d(marker_observations,relative_points),sum(observation_weights))
else:
for i in xrange(n):
marker = marker_ids[i]
Tclink = camera_link_transforms[i]
Tmlink = marker_link_transforms[i]
obs = marker_observations[i]
Trel = se3.mul(se3.inv(Tclink),se3.mul(Tmlink,new_marker_transforms[marker].average))
estimate = se3.mul(Trel,se3.inv(obs))
new_camera_transform.add(estimate,observation_weights[i])
#print " ESTIMATED CAMERA TRANSFORMS:",new_camera_transform.average
#3. compute difference between last and current estimates
diff = 0.0
diff += vectorops.normSquared(se3.error(camera_transform,new_camera_transform.average))
for marker in marker_id_list:
if marker_types[marker]=='t':
diff += vectorops.normSquared(se3.error(marker_transforms[marker],new_marker_transforms[marker].average))
else:
diff += vectorops.distanceSquared(marker_transforms[marker],new_marker_transforms[marker].average)
camera_transform = new_camera_transform.average
for marker in marker_id_list:
marker_transforms[marker] = new_marker_transforms[marker].average
if math.sqrt(diff) < tolerance:
#converged!
print "Converged with diff %g on iteration %d"%(math.sqrt(diff),iters)
break
print " ESTIMATE CHANGE:",math.sqrt(diff)
error = 0.0
for i in xrange(n):
marker = marker_ids[i]
Tclink = camera_link_transforms[i]
Tmlink = marker_link_transforms[i]
obs = marker_observations[i]
Tc = se3.mul(Tclink,camera_transform)
if marker_types[marker] == 't':
Tm = se3.mul(Tmlink,marker_transforms[marker])
error += vectorops.normSquared(se3.error(se3.mul(Tc,obs),Tm))
else:
Tm = se3.apply(Tmlink,marker_transforms[marker])
error += vectorops.distanceSquared(se3.apply(Tc,obs),Tm)
print " OBSERVATION ERROR:",math.sqrt(error)
#raw_input()
if DO_VISUALIZATION:
rgroup.setFrameCoordinates("camera estimate",camera_transform)
rgroup.setFrameCoordinates("marker estimate",marker_transforms.values()[0])
for i,obs in enumerate(marker_observations):
rgroup.setFrameCoordinates("obs"+str(i)+" estimate",obs)
visualization.add("coordinates",rgroup)
visualization.dialog()
if math.sqrt(diff) >= tolerance:
print "Max iters reached"
error = 0.0
for i in xrange(n):
marker = marker_ids[i]
Tclink = camera_link_transforms[i]
Tmlink = marker_link_transforms[i]
obs = marker_observations[i]
Tc = se3.mul(Tclink,camera_transform)
if marker_types[marker] == 't':
Tm = se3.mul(Tmlink,marker_transforms[marker])
error += vectorops.normSquared(se3.error(se3.mul(Tc,obs),Tm))
else:
Tm = se3.apply(Tmlink,marker_transforms[marker])
error += vectorops.distanceSquared(se3.apply(Tc,obs),Tm)
return (math.sqrt(error),camera_transform,marker_transforms)
def __init__(self):
self.Tsensor = se3.identity()
self.fov = 90
self.w,self.h = 320,240
self.dmin,self.dmax = 0.01,5
self.pixelError = 0.5
# -.controller.task
#
# reads in from system state
# - .command.qcmd
# - .controller.traj_q_end
# - .robot.endEffectors.0.dest_xform
# - .robot.endEffectors.1.dest_xform
modes = ['normal', 'scaling', 'gripper']
defaultDeviceState = {
'mode': 'normal',
'buttonDown': False,
'moveStartDeviceTransform': None,
'moveStartWidgetTransform': None,
'widgetTransform': se3.identity(),
'rotationScale': 1,
'positionScale': 3
}
deviceState = []
#device +z is gripper -z
#device +x is gripper -y
#device +y is gripper -x
deviceToWidgetTransform = ([0, -1, 0, -1, 0, 0, 0, 0, -1], [0, 0, 0])
deviceToBaxterBaseTransform = ([0, -1, 0, 0, 0, 1, -1, 0, 0], [0, 0, 0])
endEffectorIndices = [25, 45]
#widget coordinates in local frame of end effector
endEffectorLocalTransforms = [(so3.identity(), (0, 0, 0.08)),
def _localizeRollodexCups(self, world, color, clean_cloud_aligned, depth_uv, clean_mask, object_mask):
# copy the uv map because it is modified below
# also flip x/y since numpy is row/column
uv = depth_uv[:,:,::-1].copy()
# scale the uv coordinates by the source image size
for i in range(2):
uv[:,:,i] = numpy.clip(uv[:,:,i] * color.shape[i], 0, color.shape[i]-1)
# round to the nearest pixel
uv = uv.round().astype(numpy.int)
# filter the uv mapping using the bounding box coordinates
((bb_top, bb_left), (bb_bottom, bb_right)) = match_rollodex_template(color)
from scipy.misc import imsave
imsave('/tmp/cups.png', color[bb_top:bb_bottom, bb_left:bb_right])
logger.debug('rollodex cups template match bounding box: {}'.format(((bb_top, bb_left), (bb_bottom, bb_right))))
cups_mask = reduce(numpy.bitwise_and, [ uv[:,:,0] > bb_top, uv[:,:,0] < bb_bottom, uv[:,:,1] > bb_left, uv[:,:,1] < bb_right ])
#imsave('/tmp/cups_uv.png', uv)
imsave('/tmp/cups_mask.png', cups_mask)
valid_depth_pixels = numpy.count_nonzero(cups_mask)
if valid_depth_pixels == 0:
logger.error('rollodex cups template has no valid depth pixels')
return None
logger.debug('rollodex cups template has {} valid depth pixels'.format(valid_depth_pixels))
# get the point cloud corresponding to the cups bounding box
mask = object_mask & cups_mask
cups_cloud = clean_cloud_aligned[mask[clean_mask]]
cups_front = cups_cloud[:,0].min()
cups_mean = cups_cloud.mean(axis=0)
known_cups_width = 0.10 - 0.02
known_cups_height = 0.13 - 0.02
# generate a fake point cloud for the cups
n = 32
fake_cloud = numpy.dstack((
cups_front * numpy.ones((n, n)),
numpy.dstack(numpy.meshgrid(
known_cups_width * numpy.linspace(-0.5, 0.5, n) + cups_mean[1],
known_cups_height * numpy.linspace(-0.5, 0.5, n) + cups_mean[2],
indexing='ij'
))
))
xform = se3.identity()
object_cloud = fake_cloud.reshape((-1, 3))
(target_bin, target_object) = (self.knowledge_base.target_bin, self.knowledge_base.target_object)
bin_contents = self.knowledge_base.bin_contents[target_bin] if target_bin in self.knowledge_base.bin_contents else []
confusion_row = dict([ (obj, [0, 1][obj == 'rollodex_mesh_collection_jumbo_pencil_cup']) for obj in bin_contents ])
logger.info('confusion row for {}: {}'.format(target_object, confusion_row))
object_cloud_color = [ (p + [ [0, 0, 0] ]) for p in object_cloud.tolist() ]
#debug_cloud([ object_cloud_color ], [ (so3.identity(), cups_mean) ], world=world)
pcd.write(object_cloud_color, open('/tmp/{}_{}.pcd'.format(target_object, target_bin), 'w'))
return (xform, object_cloud_color, confusion_row)
),
try:
parts = str(raw_input()).split(' ')
except (EOFError, KeyboardInterrupt):
print
break
try:
cmd = parts[0]
args = parts[1:]
if cmd == 'load' and len(args) == 1:
kb.target_object = args[0]
kb.object_xforms[kb.target_object] = se3.identity()
elif cmd == 'move' and len(args) <= 3:
(x, y, z) = map(float, args) + ([0] * (3 - len(args)))
xform = kb.object_xforms[kb.target_object]
kb.object_xforms[kb.target_object] = (xform[0], [ x, y, z ])
elif cmd == 'mover' and len(args) <= 3:
(x, y, z) = map(float, args) + ([0] * (3 - len(args)))
xform = kb.object_xforms[kb.target_object]
kb.object_xforms[kb.target_object] = (xform[0], vectorops.add(xform[1], [ x, y, z ]))
elif cmd == 'rot' and len(args) <= 3:
(rx, ry, rz) = map(lambda a: float(a) * 3.141592/180, args) + ([0] * (3 - len(args)))
xform = kb.object_xforms[kb.target_object]
kb.object_xforms[kb.target_object] = (
def localizeSpecificObject(self, bin, object):
SHELF_DIMS_PATH = os.path.join('kb', 'shelf_dims.json')
SHELF_CLOUD_PATH = os.path.join('perception', 'segmentation', 'models', 'shelf_thin_10000.npy')
# acquire the camera image
address = { 'left': '192.168.0.103', 'right': '192.168.0.104' }[self.knowledge_base.active_limb]
#address = { 'left': '192.168.0.103', 'right': '192.168.0.103' }[self.knowledge_base.active_limb]
camera = RemoteCamera(address, xform=self.camera_xform)
camera.read()
camera.close()
# load the shelf subtraction data
bin_bounds = json.load(open(SHELF_DIMS_PATH))[bin]
# load and transform the shelf point cloud
shelf_cloud = numpy.load(open(SHELF_CLOUD_PATH))
shelf_cloud = shelf_cloud.dot(numpy.array(self.knowledge_base.shelf_xform[0]).reshape((3,3))) + self.knowledge_base.shelf_xform[1]
# load and transform the shelf model
world = WorldModel()
shelf = world.loadRigidObject(os.path.join('klampt_models', 'north_shelf', 'shelf_with_bins.obj'))
shelf.setTransform(*self.knowledge_base.shelf_xform)
# clean up the point cloud
(clean_mask, clean_cloud) = camera.clean()
_, clean_cloud_aligned, _, clean_object_mask = shelf_subtract(shelf_cloud, clean_cloud, shelf, bin_bounds, downsample=1000)
object_mask = numpy.zeros(camera.cloud.shape[:2], dtype=numpy.bool)
object_mask[clean_mask] = clean_object_mask
#clean_cloud_aligned_color = map(lambda x: x[0] + [ x[1] ], zip(clean_cloud_aligned[clean_object_mask].tolist(), camera.colorize()[clean_mask][clean_object_mask].tolist()))
#debug_cloud([ clean_cloud_aligned_color ], [ self.base_xform, self.camera_xform ])
#debug_cloud([ shelf_cloud, clean_cloud, clean_cloud_aligned ])
#pcd.write(clean_cloud_aligned_color, open('/tmp/{}_{}.pcd'.format(self.knowledge_base.target_object, self.knowledge_base.target_bin), 'w'))
(target_bin, target_object) = (self.knowledge_base.target_bin, self.knowledge_base.target_object)
bin_contents = self.knowledge_base.bin_contents[target_bin] if target_bin in self.knowledge_base.bin_contents else []
# perform special handling
if target_object == 'rollodex_mesh_collection_jumbo_pencil_cup':
logger.info('running specialized rollodex cups detection')
return self._localizeRollodexCups(world, camera.color, clean_cloud_aligned, camera.depth_uv, clean_mask, object_mask)
# dilate the object mask
# from scipy.misc import imsave
from scipy.ndimage.morphology import binary_dilation
object_mask_dilated = binary_dilation(object_mask, iterations=2)
# imsave('/tmp/test.png', object_mask)
# label connected components
(object_labeled, label_count) = distance_label(camera.cloud, object_mask_dilated, threshold=0.01)
logger.info('found {} connected components'.format(label_count))
#KH: added to help debugging without changing order bin contents
if object not in bin_contents:
logger.warning("Warning, trying to detect object "+object+" not in bin contents, adding a temporary entry")
bin_contents = bin_contents + [object]
bin_contents_with_shelf = bin_contents + [ 'shelf' ]
if not label_count:
logger.warning('found no connected components')
mask = object_mask
bin_contents = self.knowledge_base.bin_contents[self.knowledge_base.target_bin]
confusion_row = dict(zip(bin_contents, [0]*len(bin_contents)))
else:
object_blobs = sorted([ object_labeled == (l+1) for l in range(label_count) ], key=lambda b: -numpy.count_nonzero(b))
#for (i, blob) in enumerate(object_blobs):
# logger.debug('blob {}: {} points'.format(i, numpy.count_nonzero(blob)))
# filter out blobs with fewer than 1000 points
min_point_count = 1000
object_blobs = filter(lambda blob: numpy.count_nonzero(blob[clean_mask]) >= min_point_count, object_blobs)
#debug_cloud([ clean_cloud_aligned[(object_mask & blob_mask)[clean_mask]] for blob_mask in object_blobs ], [ self.base_xform, self.camera_xform ], wait=False)
known_object_count = len(bin_contents)
if known_object_count == 0:
# return all large blobs
n = len(object_blobs)
# take only the largest n blobs
blob_mask = reduce(numpy.bitwise_or, object_blobs[:n])
mask = object_mask & blob_mask
logger.warning('no objects in bin... -> returning all blobs')
else:
# load the matching statistics
match_stats = json.load(open(os.path.join('perception', 'segmentation', 'match_stats.json')))
# load the object hue histograms
known_histograms = {}
for obj in bin_contents_with_shelf:
#array = numpy.load(os.path.join('perception', 'hue_array', '{}.npz'.format(obj)))['arr_0']
#known_histograms[obj] = numpy.histogram(array, bins=range(0,181), density=True)[0].reshape(-1,1).astype(numpy.float32)
known_histograms[obj] = numpy.load(os.path.join('perception', 'uv_hist2', '{}.npz'.format(obj)))['arr_0']
# compute the color validity mask
color_valid_mask = ~invalid_mask(camera.depth_uv)
#KH addition 5/23: test to see whether blobs should be broken up
if len(object_blobs)==1 and len(known_histograms) > 2:
logger.info("Trying KMeans segmentation to break up large blob")
blob_mask = reduce(numpy.bitwise_or, object_blobs)
mask = object_mask & blob_mask & color_valid_mask
labels,k,score = xyzrgb_segment_kmeans(camera.cloud[mask],camera.colorize()[mask],known_histograms.values())
labels = numpy.array(labels)
object_blobs = []
for i in xrange(len(known_histograms)):
blank = numpy.zeros_like(object_mask,dtype=numpy.bool)
blank[mask] = (labels==i)
object_blobs.append(blank)
# score each blob for each object in the bin
blob_scores = []
blob_accepts = []
blob_rejects = []
matched_blobs = {}
confusion_matrix = []
for (i, blob) in enumerate(object_blobs):
# compute the blob histogram
blob_uv = [ rgb2yuv(*rgb)[1:3] for rgb in camera.colorize()[object_mask & blob & color_valid_mask] ]
blob_histogram = make_uv_hist(blob_uv)
# compare the blob to each possible object
scores = dict([ (obj, 2 * numpy.minimum(blob_histogram, histogram).sum() - numpy.maximum(blob_histogram, histogram).sum()) for (obj, histogram) in known_histograms.items() ])
logger.debug('blob {}:'.format(i))
blob_scores.append(scores)
logger.debug(' scores: {}'.format([ '{}={}'.format(*x) for x in scores.items() ]))
# apply the cutoff to each score and associate with positive confidence
accepts = dict([ (obj, match_stats[obj]['ppv']) for obj in bin_contents_with_shelf if scores[obj] >= match_stats[obj]['cutoff'] ])
blob_accepts.append(accepts)
logger.debug(' accepts: {}'.format([ '{}={}'.format(*x) for x in accepts.items() ]))
# apply the cutoff to each score and associate with negative confidence
rejects = dict([ (obj, match_stats[obj]['npv']) for obj in bin_contents_with_shelf if scores[obj] < match_stats[obj]['cutoff'] ])
blob_rejects.append(rejects)
logger.debug(' rejects: {}'.format([ '{}={}'.format(*x) for x in rejects.items() ]))
# populate the confusion matrix
shelf_probability = 0.1
S = lambda o: match_stats[o]['specificity']
iS = lambda o: 1 - S(o)
total_probability = sum(map(S, accepts)) + sum(map(iS, rejects)) + shelf_probability
confusion_matrix.append([ [ iS(o), S(o) ][ o in accepts ] / total_probability for o in bin_contents ])
# resolve multiple assignments
if len(accepts) > 1:
# choose the object with the highest matched confidence
best_match_object = sorted(accepts.items(), key=lambda a: -a[1])[0][0]
# remove all other objects
for (obj, confidence) in accepts.items():
if obj != best_match_object:
del accepts[obj]
logger.warn('blob {} multiple accept ignored: {}={}'.format(i, obj, confidence))
if len(accepts) == 1:
# a single match so record it
matched_blobs.setdefault(accepts.keys()[0], []).append(i)
logger.info('blob {} assigned to {} by single match'.format(i, accepts.keys()[0]))
confusion_matrix = numpy.array(confusion_matrix)
logger.debug('confusion matrix:\n{}'.format(numpy.array(confusion_matrix)))
# assign blobs by least threshold difference until all objects are assigned (excluding shelf) or all blobs are assigned
while len([ k for k in matched_blobs.keys() if k != 'shelf']) < len(bin_contents) and len(sum(matched_blobs.values(), [])) < len(object_blobs):
best_match_objects = []
for (i, scores) in enumerate(blob_scores):
# only consider unmatched blobs
if i not in sum(matched_blobs.values(), []):
threshold_differences = [ [obj, match_stats[obj]['cutoff'] - score] for (obj, score) in scores.items() ]
best_match_objects.append([ i ] + sorted(threshold_differences, key=lambda d: d[1])[0])
# choose the least threshold difference across all blobs and objects
best_blob, best_object, _ = sorted(best_match_objects, key=lambda b: b[2])[0]
matched_blobs.setdefault(best_object, []).append(best_blob)
logger.warn('blob {} assigned to {} by least threshold difference'.format(best_blob, best_object))
# report unmatched blobs and objects (excluding shelf)
for obj in bin_contents:
if obj not in matched_blobs:
logger.warn('no blobs matched {}'.format(obj))
for i in range(len(object_blobs)):
if i not in sum(matched_blobs.values(), []):
logger.warn('blob {} is unassigned'.format(i))
for obj in bin_contents_with_shelf:
if obj in matched_blobs:
print obj
mask = reduce(numpy.bitwise_or, [ object_blobs[i] for i in matched_blobs[obj] ], numpy.zeros_like(object_mask))
object_cloud = clean_cloud_aligned[mask[clean_mask]]
xform = (so3.identity(), object_cloud.mean(axis=0).tolist())
object_cloud_color = map(lambda x: x[0] + [ x[1] ], zip(object_cloud.tolist(), camera.colorize()[mask].tolist()))
#debug_cloud([ object_cloud_color ], [ self.base_xform, self.camera_xform, xform ])
# check that the target object was found
if target_object not in matched_blobs:
logger.error('no blobs assigned to target object')
return None
# return blob for the selected object
mask = reduce(numpy.bitwise_or, [ object_blobs[i] for i in matched_blobs[target_object] ], numpy.zeros_like(object_mask))
# return the confusion matrix rows for the selected object
confusion_rows = confusion_matrix[matched_blobs[target_object], :]
confusion_row = dict([ (obj, confusion_rows[:, i].mean()) for (i, obj) in enumerate(bin_contents) ])
logger.info('confusion row for {}: {}'.format(target_object, confusion_row))
object_cloud = clean_cloud_aligned[mask[clean_mask]]
#xform = (so3.identity(), object_cloud.mean(axis=0).tolist())
xform = se3.identity()
object_cloud_color = map(lambda x: x[0] + [ x[1] ], zip(object_cloud.tolist(), camera.colorize()[mask].tolist()))
#debug_cloud([ object_cloud_color ], [ self.base_xform, self.camera_xform, xform ])
# downsample the cloud to about 1000 points
ratio = int(len(object_cloud) / 1000)
if ratio > 1:
object_cloud_color = object_cloud_color[::ratio]
pcd.write(object_cloud_color, open('/tmp/{}_{}.pcd'.format(self.knowledge_base.target_object, self.knowledge_base.target_bin), 'w'))
return (xform, object_cloud_color, confusion_row)
grippermodules = {'reflex':reflex_col,
'rethink_electric_gripper':rethink_electric_gripper,
}
grippermodule = grippermodules[grippername]
world = WorldModel()
world.readFile(os.path.join('../klampt_models/',grippermodule.klampt_model_name))
robot = world.robot(0)
#load the items and spawn one in the world
apc.load_item(objectname,gripper=grippername)
item = apc.item_info[objectname]
item.geometry = apc.load_item_geometry(item)
itemposed = apc.ItemInBin(item,'blah')
itemposed.xform = se3.identity()
apc.spawn_item_in_world(itemposed,world)
#set the directory
print "Resouce directory is",os.path.join('../resources/',os.path.splitext(grippermodule.klampt_model_name)[0])
resource.setDirectory(os.path.join('../resources/',os.path.splitext(grippermodule.klampt_model_name)[0]))
#edit the default configurations
default_open_config = resource.get('default_open.config',
description='Default hand open configuration',
doedit='auto',
world=world)
default_closed_config = resource.get('default_closed.config',
description='Default hand closed configuration',
doedit='auto',
world=world)
def icp(object,scene,reuse=True):
"""Computes a rotation and translation of the object to the scene
using the ICP algorithm. Returns the transform (R,t) in Klamp't se3
format.
"""
extract_resolution = 0.01
(obj, obj_connectivity) = clean_cloud(numpy.array(object), 0.01)
print 'sampled object', 100*extract_resolution, 'cm:', len(object), '->', len(obj)
# get the object properties
obj_dims = numpy.amax(object, axis=0) - numpy.amin(object, axis=0)
obj_volume = numpy.count_nonzero(obj_connectivity) * extract_resolution**3
print 'object', '%.1fx%.1fx%.1f' % tuple(100*obj_dims), 'cm', '(%.1f' % (100**3*obj_volume,), 'cm^3)'
# this caching code is to make detect_3d run much faster
# otherwise it would need to run connected components each time which is slow
global cached
if reuse and cached:
(scene, scene_tree, grids, scene_connectivity, resolution) = cached
else:
# fast point cloud lookups
print 'generate scene kdtree'
scene = numpy.array(scene)
scene_tree = scipy.spatial.KDTree(scene[::])
# run connected components
(grids, scene_connectivity, resolution) = multiscale_connected_components(scene, scene_tree, 0.10, [1, 4, 8, 16])
# cache the results
cached = (scene, scene_tree, grids, scene_connectivity, resolution)
inflated_resolution = resolution*2**0.5
# thin the connectivity grids
# this is needed to separate the kygen_eggs and kong_duck from the table
# unfortunately, it also destroys the highlighters...
for i in range(6):
scene_connectivity = thin_connectivity(scene_connectivity)
labeled, label_count = scipy.ndimage.label(scene_connectivity)
candidates = []
# compute candidate coarse bounds
index_bounds = scipy.ndimage.find_objects(labeled)
for label in range(label_count):
ib = index_bounds[label]
lbounds = numpy.array([ grids[i][ib[0].start, ib[1].start, ib[2].start] for i in range(3) ]) - inflated_resolution
ubounds = numpy.array([ grids[i][ib[0].stop-1, ib[1].stop-1, ib[2].stop-1] for i in range(3) ]) + inflated_resolution
# compute rough dimensions and volume
dim = ubounds - lbounds
rough_volume = numpy.count_nonzero(labeled == label) * resolution**3
candidates.append({ 'label': label+1,
'rough_bounds': numpy.vstack((lbounds, ubounds)),
'rough_volume': rough_volume
})
print 'found candidate', '~%.1fx%.1fx%.1f' % tuple(100*dim), 'cm', '(~%.1f' % (100**3*rough_volume,), 'cm^3)'
if len(candidates) == 0:
print 'no candidates found!'
return se3.identity, float('inf')
# sort candidates such that the one with estimated volume closest to the target object is matched first
candidates.sort(key=lambda c: abs(c['rough_volume'] - obj_volume))
# extra candidate point clouds and get finer bounds
for candidate in candidates:
# reject large candidates before extraction to save time
if candidate['rough_volume'] > obj_volume * 10:
print 'candidate too large for good match -- skipping'
continue
(candidate['points'], candidate['connectivity']) = extract_scene_candidate(scene_tree, grids, labeled == candidate['label'], resolution, extract_resolution)
ubounds = numpy.amax(candidate['points'], axis=0)
lbounds = numpy.amin(candidate['points'], axis=0)
# compute fine dimensions and volume
dim = ubounds - lbounds
fine_volume = numpy.count_nonzero(candidate['connectivity']) * extract_resolution**3
candidate['fine_bounds'] = numpy.vstack((lbounds, ubounds))
candidate['fine_volume'] = fine_volume
print 'extracted candidate', 100*extract_resolution, 'cm:', '%.1fx%.1fx%.1f' % tuple(100*dim), 'cm', '(%.1f' % (100**3*fine_volume,), 'cm^3)'
if fine_volume < obj_volume / 10:
print 'candidate too small for good match -- skipping'
continue
# time to attempt a match to this candidate
R, t, metric = match_candidate(obj, scipy.spatial.KDTree(candidate['points']), 100)
# save results for later
candidate.update({ 'transform': (R.T.flat[:], t[0]), # needed output format
'metric': metric
})
if metric < 1:
print 'good enough fit found:', metric
return candidate['transform'], metric #[ c.get('fine_bounds', c['rough_bounds']) for c in candidates ]
else:
print 'candidate is poor fit -- trying another'
# no good candidates found so just pick the one with the best metric
candidates.sort(key=lambda c: c.get('metric', float('inf')))
best_candidate = candidates[0]
print 'best fit:', best_candidate.get('metric', float('inf'))
return best_candidate.get('transform', se3.identity()), best_candidate.get('metric', float('inf'))#, [ c.get('fine_bounds', c['rough_bounds']) for c in candidates ]
