def sphere_collision_tests(x, radius, grid):
bmin = vectorops.sub(x, [radius] * len(x))
bmax = vectorops.add(x, [radius] * len(x))
imin = [max(0, int(v)) for v in bmin]
imax = [min(int(v), b - 1) for v, b in zip(bmax, workspace_size)]
for i in range(imin[0], imax[0]):
for j in range(imin[1], imax[1]):
for k in range(imin[2], imax[2]):
if vectorops.distanceSquared((i, j, k), x) < radius**2:
yield (i, j, k)
return
def get_ik_solutions(self,goals,limbs,initialConfig=None,maxResults=10,maxIters=100,tol=1e-3,validity_checker=None):
"""Given a list of goals and their associated limbs, returns a list
of (q,index) pairs, where q is an IK solution for goals[index].
The results are sorted by distance from initialConfig.
Arguments:
- goals: a list of IKObjectives
- limbs: a list of 'left'/'right', one for each goal, corresponding
to the limb that the goal is defined on
- initialConfig: optionally, a configuration for the IK seed
- maxResults: the maximum number of IK results to return
- maxIters: the maximum number of random samples to draw
- tol: the ik solving tolerance
- validity_checker: optionally, a special collision checker f(limb)
that returns True if the robot's current limb configuration is valid.
If None, the standard planner collision checker is used.
Returns: a list [(solution1,index1),...,(solutionn,indexn)] of up to
maxResults collision-free solutions.
"""
if initialConfig == None:
initialConfig = self.controller.getCommandedConfig()
if validity_checker == None:
validity_checker = self.planner.check_collision_free
numTrials = [0]*len(goals)
ikSolutions = []
for i in range(maxIters):
index = random.randint(0,len(goals)-1)
goal = goals[index]
limb = limbs[index]
numTrials[index] += 1
if numTrials[index] == 1:
self.robot.setConfig(initialConfig)
else:
self.randomize_limb_position(limb,center=initialConfig,range=0.05*(numTrials[index]-1))
if ik.solve(goal,tol=tol):
if validity_checker(limb):
ikSolutions.append((self.robot.getConfig(),index))
if len(ikSolutions) >= maxResults: break
else:
print "IK solution for goal",index,"was in collision, trying again"
if len(ikSolutions)==0:
print "No collision free IK solution"
return []
sortedSolutions = sorted([(vectorops.distanceSquared(solution[0],initialConfig),solution) for solution in ikSolutions])
return [s[1] for s in sortedSolutions]
def get_ik_solutions(self,goal,maxResults=10,maxIters=1000,tol=1e-3,printer=True):
initialConfig = self.controller.getCommandedConfig()
validity_checker = self.planner.check_collision_free
numTrials = 0
ikSolutions = []
numSolutions = 0
numColFreeSolutions = 0
for i in range(1):
numTrials += 1
# if first time trying the ik goal, initialize with current config
if numTrials == 1:
self.robot.setConfig(initialConfig)
# else, initialize with a random q, incrementally perturbing more from inital config
else:
self.robot.setConfig(self.randomPose())
#print self.robot.getConfig()
if ik.solve(goal,tol=tol):
numSolutions += 1
if validity_checker():
numColFreeSolutions += 1
ikSolutions.append(self.robot.getConfig())
if len(ikSolutions) >= maxResults: break
if printer:
print "< IK Summary >",
print "Attempted:", numTrials, "/", maxIters, "; Result:", numColFreeSolutions, "/", numSolutions, "col. free. solutions"
# sort solutions by distance to initial config
sortedSolutions = []
for solution in ikSolutions:
dist = vectorops.distanceSquared(solution,initialConfig)
sortedSolutions.append( (dist, solution) )
sortedSolutions = sorted(sortedSolutions, key=itemgetter(0))
# s[0] contains the distance-sqaured values
# s[1] contains the ikSolution
# to return a sorted list...
# return [s[1] for s in sortedSolutions]
# to return the shorted-distance solution...
return sortedSolutions[0][1]
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)
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 mahalanobis_distance2(u,v,A=None):
if A is None:
return vectorops.distanceSquared(u,v)
else:
z = np.array(vectorops.sub(u,v))
return np.dot(z.T,np.dot(A,z))