def getCommandVelocity(self, q, dq, dt): """Gets the command velocity from the current state of the robot. """ eP = self.getSensedError(q) #vcmd = hP*eP + hD*eV + hI*eI vP = gen_mul(self.hP, eP) vcmd = vP #D term vcur = self.getSensedVelocity(q, dq, dt) eD = None if vcur != None: eD = vectorops.sub(vcur, self.dxdes) vD = gen_mul(self.hD, eD) vcmd = vectorops.add(vcmd, vD) #I term if self.eI != None: vI = gen_mul(self.hI, self.eI) vcmd = vectorops.add(vcmd, vI) #print "task",self.name,"error P=",eP,"D=",eD,"E=",self.eI #do acceleration limiting if vcur != None: dvcmd = vectorops.div(vectorops.sub(vcmd, vcur), dt) dvnorm = vectorops.norm(dvcmd) if dvnorm > self.dvcmdmax: vcmd = vectorops.madd(vcur, dvcmd, self.dvcmdmax / dvnorm * dt) print self.name, "acceleration limited by factor", self.dvcmdmax / dvnorm * dt, "to", vcmd #do velocity limiting vnorm = vectorops.norm(vcmd) if vnorm > self.vcmdmax: vcmd = vectorops.mul(vcmd, self.vcmdmax / vnorm) print self.name, "velocity limited by factor", self.vcmdmax / vnorm, "to", vcmd return vcmd
def angle(a, b): anorm = vectorops.norm(a) bnorm = vectorops.norm(b) if anorm < 1e-6 or bnorm < 1e-6: return 0 dp = vectorops.dot(vectorops.div(a, anorm), vectorops.div(b, bnorm)) dp = max(min(dp, 1), -1) return math.acos(dp)
def getCommandVelocity(self, q, dq, dt): """Gets the command velocity from the current state of the robot. """ eP = self.getSensedError(q) #vcmd = hP*eP + hD*eV + hI*eI vP = gen_mul(self.hP,eP) vcmd = vP #D term vcur = self.getSensedVelocity(q,dq,dt) eD = None if vcur != None: eD = vectorops.sub(vcur, self.dxdes) vD = gen_mul(self.hD,eD) vcmd = vectorops.add(vcmd, vD) #I term if self.eI != None: vI = gen_mul(self.hI,self.eI) vcmd = vectorops.add(vcmd, vI) #print "task",self.name,"error P=",eP,"D=",eD,"E=",self.eI #do acceleration limiting if vcur != None: dvcmd = vectorops.div(vectorops.sub(vcmd,vcur),dt) dvnorm = vectorops.norm(dvcmd) if dvnorm > self.dvcmdmax: vcmd = vectorops.madd(vcur,dvcmd,self.dvcmdmax/dvnorm*dt) print self.name,"acceleration limited by factor",self.dvcmdmax/dvnorm*dt,"to",vcmd #do velocity limiting vnorm = vectorops.norm(vcmd) if vnorm > self.vcmdmax: vcmd = vectorops.mul(vcmd,self.vcmdmax/vnorm) print self.name,"velocity limited by factor",self.vcmdmax/vnorm,"to",vcmd return vcmd
def right_arm_ik_near_seed(self, right_target, ignore_elbow_up_constraint=True): """Solves IK to move the right arm to the specified right_target ([x, y, z] in world space) """ self.load_real_robot_state() self.set_model_right_arm(eval("Q_IK_SEED_" + self.current_bin)) oldRSquared = -1 q_ik = None qmin, qmax = self.robotModel.getJointLimits() for i in range(1000): point2_local = vectorops.add(VACUUM_POINT_XFORM[1], [0.5, 0, 0]) point2_world = vectorops.add(right_target, [0, 0, -0.5]) goal1 = ik.objective(self.robotModel.link("right_wrist"), local=VACUUM_POINT_XFORM[1], world=right_target) goal2 = ik.objective(self.robotModel.link("right_wrist"), local=point2_local, world=point2_world) if ik.solve([goal1, goal2], tol=0.0001) and (self.elbow_up() or ignore_elbow_up_constraint): q_vals = [self.robotModel.getConfig()[v] for v in self.right_arm_indices] rSquared = vectorops.norm(vectorops.sub(q_vals, eval("Q_IK_SEED_" + self.current_bin))) if oldRSquared < 0 or oldRSquared > rSquared: oldRSquared = rSquared q_ik = q_vals print FAIL_COLOR + "right_arm_ik failed for " + str(right_target) + END_COLOR if not (self.elbow_up() or ignore_elbow_up_constraint): print FAIL_COLOR + str([self.robotModel.getConfig()[v] for v in self.right_arm_indices]) + END_COLOR print FAIL_COLOR + "IK found but elbow wasn't up" + END_COLOR if oldRSquared >= 0: self.set_model_right_arm(q_ik) return True return False
def solve_2R_inverse_kinematics(x,y,L1=1,L2=1): """For a 2R arm centered at the origin, solves for the joint angles (q1,q2) that places the end effector at (x,y). The result is a list of up to 2 solutions, e.g. [(q1,q2),(q1',q2')]. """ D = vectorops.norm((x,y)) thetades = math.atan2(y,x) if D == 0: raise ValueError("(x,y) at origin, infinite # of solutions") c2 = (D**2-L1**2-L2**2)/(2.0*L1*L2) q2s = [] if c2 < -1: print "solve_2R_inverse_kinematics: (x,y) inside inner circle" return [] elif c2 > 1: print "solve_2R_inverse_kinematics: (x,y) out of reach" return [] else: if c2 == 1: q2s = [math.acos(c2)] else: q2s = [math.acos(c2),-math.acos(c2)] res = [] for q2 in q2s: thetaactual = math.atan2(math.sin(q2),L1+L2*math.cos(q2)) q1 = thetades - thetaactual res.append((q1,q2)) return res
def _solveIK(self, coord): """ Returns the degrees for the pan and tilt motor and the inches for the linear actuator needed to reach the point [x,y,z]. Args: coord - cartesian coordinates [x,y,z] in PPU base frame in inches Returns: PPU configuration [q1,q2,q3] to reach the x,y,z coordinates, in degrees and inches where q1 is degree angle of pan (base) motor q2 is degree angle of tilt motor q3 is extension of linear actuator in inches """ xw, yw, zw = coord y = yw x = xw + PPU.xb z = zw - PPU.hb q3 = vectorops.norm((x, y, z)) q2 = math.degrees(math.asin(-z / q3)) if q2 == 90 or q2 == -90: q1 = 0 #infinite solutions else: q1 = math.degrees(math.atan2(y, x)) if (q1 < PPU.q1LL) or (q1 > PPU.q1UL): raise ValueError("q1 is %f, which is not within IK joint limits." % q1) elif (q2 < PPU.q2LL) or (q2 > PPU.q2UL): raise ValueError("q2 is %f, which is not within IK joint limits." % q2) elif (q3 < PPU.q3LL) or (q3 > PPU.q3UL): raise ValueError("q3 is %f, which is not within IK joint limits." % q3) else: return [q1, q2, q3]
def solve_2R_inverse_kinematics(x, y, L1=1, L2=1): """For a 2R arm centered at the origin, solves for the joint angles (q1,q2) that places the end effector at (x,y). The result is a list of up to 2 solutions, e.g. [(q1,q2),(q1',q2')]. """ D = vectorops.norm((x, y)) thetades = math.atan2(y, x) if D == 0: raise ValueError("(x,y) at origin, infinite # of solutions") c2 = (D**2 - L1**2 - L2**2) / (2.0 * L1 * L2) q2s = [] if c2 < -1: print "solve_2R_inverse_kinematics: (x,y) inside inner circle" return [] elif c2 > 1: print "solve_2R_inverse_kinematics: (x,y) out of reach" return [] else: if c2 == 1: q2s = [math.acos(c2)] else: q2s = [math.acos(c2), -math.acos(c2)] res = [] for q2 in q2s: thetaactual = math.atan2(math.sin(q2), L1 + L2 * math.cos(q2)) q1 = thetades - thetaactual res.append((q1, q2)) return res
def printStatus(self,q): """Prints a status printout summarizing all tasks' errors.""" priorities = set() names = dict() errors = dict() totalerrors = dict() for t in self.taskList: if t.weight==0: continue priorities.add(t.level) s = t.name if len(s) > 8: s = s[0:8] err = t.getSensedError(q) names.setdefault(t.level,[]).append(s) errors.setdefault(t.level,[]).append("%.3f"%(vectorops.norm(err)),) werrsq = vectorops.normSquared(vectorops.mul(err,t.weight)) totalerrors[t.level] = totalerrors.get(t.level,0.0) + werrsq cols = 5 colwidth = 10 for p in priorities: print "Priority",p,"weighted error^2",totalerrors[p] pnames = names[p] perrs = errors[p] start = 0 while start < len(pnames): last = min(start+cols,len(pnames)) print " Name: ", for i in range(start,last): print pnames[i],' '*(colwidth-len(pnames[i])), print print " Error: ", for i in range(start,last): print perrs[i],' '*(colwidth-len(perrs[i])), print start=last
def insideGeometry(self, point, g, upperThreshold, lowerThreshold): #t1 = time.time() #print "using rayCast" direction = vectorops.unit(point) #print g.getBB()[0][0] (hit, pt) = g.rayCast((0, 0, 0), direction) depthPoint = vectorops.norm(point) depthHit = vectorops.norm(pt) diff = depthPoint - depthHit #print "rayCast done in: ",time.time()-t1 #self.count2 = self.count2 + 1 if hit: #print "hit!!!!!!" self.count2 = self.count2 + 1 if diff < upperThreshold and diff > lowerThreshold: # If point lies inside robot body return True return False
def score(self,robot): """Returns an error score that is equal to the optimum at a feasible solution. Evaluated at the robot's current configuration.""" for obj in self.objectives: obj.robot = robot solver = ik.solver(self.objectives) c = (0 if self.costFunction is None else self.costFunction(robot.getConfig())) return c+vectorops.norm(solver.getResidual())
def interpolate_rotation(R1,R2,u): """Interpolate linearly between the two rotations R1 and R2. TODO: the current implementation doesn't work properly. Why? """ m1 = so3.moment(R1) m2 = so3.moment(R2) mu = interpolate_linear(m1,m2,u) angle = vectorops.norm(mu) axis = vectorops.div(mu,angle) return so3.rotation(axis,angle)
def interpolate_rotation(R1, R2, u): """Interpolate linearly between the two rotations R1 and R2. TODO: the current implementation doesn't work properly. Why? """ m1 = so3.moment(R1) m2 = so3.moment(R2) mu = interpolate_linear(m1, m2, u) angle = vectorops.norm(mu) axis = vectorops.div(mu, angle) return so3.rotation(axis, angle)
def robotSelfFilter(self, points, Rinv, tinv): """ Get rid of points that belong to the robot's arms""" robot = self.world.robot(0) arm_link_names = self.left_arm_link_names + self.right_arm_link_names bbMargin = 0.1 #upperThreshold = 0.1 # Need to tune #lowerThreshold = -0.1 # Need to tune armBBs = [] transformedG = [] maxArmDepth = 0 for i in arm_link_names: currentT = robot.link(i).getTransform() (newR, newt) = se3.mul((Rinv, tinv), currentT) g = robot.link(i).geometry() g.setCurrentTransform(newR, newt) transformedG.append(g) myBB = g.getBB() if myBB[1][2] > maxArmDepth: maxArmDepth = myBB[1][2] expandedBB = ((myBB[0][0] - bbMargin, myBB[0][1] - bbMargin, myBB[0][2] - bbMargin), (myBB[1][0] + bbMargin, myBB[1][1] + bbMargin, myBB[1][2] + bbMargin)) armBBs.append(expandedBB) allIndices = self.indicesOfInterest(armBBs) count = 0 self.count2 = 0 for j, indices in enumerate(allIndices): if indices == None: continue for i in indices: p = points[i] if p == None: continue x = p[0] y = p[1] z = p[2] if vectorops.norm((x, y, z)) > maxArmDepth + 0.3: continue points[i] = None continue #point = (x,y,z) #if(z<1.2) and self.insideGeometry(point,transformedG[j],upperThreshold,lowerThreshold): # count = count + 1 # points[i] = None return points
def advance(self, q, dq, dt): """ Updates internal state: accumulates iterm and updates x_last """ if self.weight > 0: eP = self.getSensedError(q) # update iterm if self.eI == None: self.eI = vectorops.mul(eP,dt) else: self.eI = vectorops.madd(self.eI, eP, dt) einorm = vectorops.norm(self.eI) if einorm > self.eImax: self.eI = vectorops.mul(self.eI,self.eImax/einorm) #update qLast self.qLast = q return
def advance(self, q, dq, dt): """ Updates internal state: accumulates iterm and updates x_last """ if self.weight > 0: eP = self.getSensedError(q) # update iterm if self.eI == None: self.eI = vectorops.mul(eP, dt) else: self.eI = vectorops.madd(self.eI, eP, dt) einorm = vectorops.norm(self.eI) if einorm > self.eImax: self.eI = vectorops.mul(self.eI, self.eImax / einorm) #update qLast self.qLast = q return
def voxelgridfilter(self, points, gridX, gridY, gridZ, num): #filtered = [] dictionary = {} for i, p in enumerate(points): x = p[0] y = p[1] z = p[2] #xangle = math.degrees(math.atan2(x,z))+35.3 #yangle = math.degrees(math.atan2(y,z)) + 30 #if xangle>self.xmax: # self.xmax = xangle #if xangle<self.xmin: # self.xmin = xangle #if yangle>self.ymax: # self.ymax = yangle #if yangle<self.ymin: # self.ymin = yangle if math.isnan(x) or vectorops.norm((x, y, z)) < 0.31: points[i] = None continue xindex = int(math.floor(x / gridX)) yindex = int(math.floor(y / gridY)) zindex = int(math.floor(z / gridZ)) key = (xindex, yindex, zindex) if key not in dictionary: dictionary[key] = [] dictionary[key].append(i) for key, value in dictionary.iteritems(): #print value if len(value) < num: for j in value: points[j] = None #print points[j] return points
def voxelgridfilter(self, points, gridX, gridY, gridZ, num): """ a simple voxelgrid filter to get rid of noise in point cloud data""" dictionary = {} for i, p in enumerate(points): x = p[0] y = p[1] z = p[2] if math.isnan(x) or vectorops.norm((x, y, z)) < 0.31: points[i] = None continue xindex = int(math.floor(x / gridX)) yindex = int(math.floor(y / gridY)) zindex = int(math.floor(z / gridZ)) key = (xindex, yindex, zindex) if key not in dictionary: dictionary[key] = [] dictionary[key].append(i) for key, value in dictionary.iteritems(): if len(value) < num: for j in value: points[j] = None return points
def callback(self, data): if self.newPC == None: data_out = pc2.read_points(data, skip_nans=True) #data_out = pc2.read_points(data) pc = PointCloud() pc.propertyNames.append('rgba') t0 = time.time() points = list(data_out) #filtered = self.radiusfilter(points,0.1,0) filtered = self.voxelgridfilter(points, 0.1, 0.1, 0.1, 10) print "Noise cleaned up in time", time.time() - t0 #filtered = points for p in filtered: coordinates = (p[0], p[1], p[2]) if vectorops.norm(coordinates) > 0.4: index = pc.addPoint(coordinates) i = struct.unpack("i", struct.pack("f", p[3]))[0] pc.setProperty(index, 0, i) #print "Point cloud copied to Klampt format in time",time.time()-t0 q = (0.566222, 0.423455, 0.424871, 0.5653) R1 = so3.from_quaternion(q) t1 = (0.228665, 0.0591513, 0.0977748) T1 = (R1, t1) R2 = so3.rotation((0, 0, 1), math.pi / 2) t2 = (0, 0, 1) T2 = (R2, t2) T = se3.mul(T2, T1) (R, t) = T pc.transform(R, t) #print "Point cloud transformed in time",time.time()-t0 self.newPC = pc else: print "Not ready yet to receive new point cloud..."
elif cmd == 'find': print master._find_object() elif cmd == 'grasp': print master._grasp_object() elif cmd == 'retrieve': print master._retrieve_object() elif cmd == 'move_cartesian' and len(args) == 4: task = None limb = args[0] amount = [float(args[1]),float(args[2]),float(args[3])] maxspeed = 0.01 command = {'limb':limb,'velocity':vectorops.div(amount,vectorops.norm(amount)/maxspeed),'duration':vectorops.norm(amount)/maxspeed} print "Trying cartesian_drive low level command" task = master.manager.control.execute([ ( 'cartesian_drive', command, 0, 0), ]) while task and not task.done: sleep(0.1) elif cmd == 'gripper' and len(args) == 1: task = None if args[0] == 'parallel': task = master.manager.control.execute([ ( 'left_gripper', [0.5, 0.5, 0.5, 1], 0, 0), ( 'right_gripper', [0.5, 0.5, 0.5, 1], 0, 0),
def robotSelfFilter(self, points, Rinv, tinv): #t1 = time.time() #filtered = [] robot = self.world.robot(0) #camPos = (0.24539, 0.0893425, 0.00112145) # Not needed anymore in camera frame arm_link_names = self.left_arm_link_names + self.right_arm_link_names bbMargin = 0.1 upperThreshold = 0.1 # Need to tune lowerThreshold = -0.1 # Need to tune armBBs = [] transformedG = [] #testPoint = (0.26839444608864566, -0.6676082722078356, 1.2409759988857147) #testPoint = (0.6351629925287235, -0.8301655827368236, 1.2909760000026105) #print se3.apply((Rinv, tinv), testPoint) maxArmDepth = 0 for i in arm_link_names: #robot.link(i).appearance().setColor(0,0,1,1) currentT = robot.link(i).getTransform() #pt = se3.apply(currentT, (0,0,0.1)) #print se3.apply((Rinv, tinv), pt) #print currentT (newR, newt) = se3.mul((Rinv, tinv), currentT) #print se3.apply((newR,newt), (0,0,0.1)) g = robot.link(i).geometry() g.setCurrentTransform(newR, newt) #g.transform(Rinv, tinv) transformedG.append(g) myBB = g.getBB() if myBB[1][2] > maxArmDepth: maxArmDepth = myBB[1][2] expandedBB = ((myBB[0][0] - bbMargin, myBB[0][1] - bbMargin, myBB[0][2] - bbMargin), (myBB[1][0] + bbMargin, myBB[1][1] + bbMargin, myBB[1][2] + bbMargin)) armBBs.append(expandedBB) #robot.link(arm_link_names[10]).appearance().setColor(0,0,1,1) #print armBBs[10] #print "Arm geometry transformed in time: ",time.time()-t1 allIndices = self.indicesOfInterest(armBBs) #for i, p in enumerate(points): #print points[15000] count = 0 self.count2 = 0 for j, indices in enumerate(allIndices): #print indices[len(indices)-1] if indices == None: continue #print len(indices) for i in indices: #print points[i][3] #if points[i] !=None: # newpoint = (points[i][0], points[i][1], points[i][2], -1.7014118346e+38) # points[i] = newpoint #continue p = points[i] if p == None: continue x = p[0] y = p[1] z = p[2] if vectorops.norm((x, y, z)) > maxArmDepth + 0.3: continue points[i] = None continue point = (x, y, z) #discard = False #for j,bb in enumerate(armBBs): #if(z<1.2): #g = robot.link(j).geometry() #g.transform(Rinv, tinv) # geometry transformed to camera frame #if(self.insideGeometryBB(point,bb,bbMargin)): # if(self.insideGeometry(point,transformedG[j],upperThreshold,lowerThreshold)): # discard = True #break #pass #if discard: # points[i] = None #print "**********************" # count2 = count2+1 if (z < 1.2) and self.insideGeometry(point, transformedG[j], upperThreshold, lowerThreshold): #print "detected" count = count + 1 points[i] = None print "detected: ", count print "hits: ", self.count2 return points
rgroup.addFrame("marker (ground truth)",parent="marker link",relativeCoordinates=Tm0) for i,(obs,obs0) in enumerate(zip(observations,trueObservations)): rgroup.addFrame("obs"+str(i)+" (ground truth)",parent="camera (ground truth)",relativeCoordinates=obs0) rgroup.addFrame("obs"+str(i)+" (from camera)",parent="camera (ground truth)",relativeCoordinates=obs) visualization.add("world",world) for i,q in enumerate(calibrationConfigs): visualization.add("config"+str(i),q) app = lc.appearance().clone() app.setColor(0.5,0.5,0.5,0.1) visualization.setAppearance("config"+str(i),app) visualization.add("simulated coordinates",rgroup) visualization.dialog() res = calibrate_robot_camera(robot,camera_link, calibrationConfigs, observations, [marker_link]*len(calibrationConfigs)) print print "Per-observation reconstruction error:",res[0]/numObs print "Estimated camera transform:",res[1] print " total error:",vectorops.norm(se3.error(res[1],Tc0)) print " rotation errors:",se3.error(res[1],Tc0)[:3] print " translation errors:",se3.error(res[1],Tc0)[3:] print "Estimated marker transform:",res[2][marker_link] print " error:",vectorops.norm(se3.error(res[2][marker_link],Tm0)) print " rotation errors:",se3.error(res[2][marker_link],Tm0)[:3] print " translation errors:",se3.error(res[2][marker_link],Tm0)[3:] visualization.kill()
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 solve(self,robot=None,params=IKSolverParams()): """Globally solves the given problem. Returns the solution configuration or None if failed.""" #set this to False if you want to run the local optimizer for each #random restart. postOptimize = True t0 = time.time() if len(self.objectives) == 0: if self.costFunction is not None or self.feasibilityTest is not None: raise NotImplementedError("Raw optimization without IK goals not done yet") return None if robot is None: if not hasattr(self.objectives[0],'robot'): print "The objectives passed to IKSolver should come from ik.objective() or have their 'robot' member manually set" robot = self.objectives[0].robot else: for obj in self.objectives: obj.robot = robot solver = ik.solver(self.objectives) if self.activeDofs is not None: solver.setActiveDofs(self.activeDofs) ikActiveDofs = self.activeDofs if self.jointLimits is not None: solver.setJointLimits(*self.jointLimits) qmin,qmax = solver.getJointLimits() if self.activeDofs is None: #need to distinguish between dofs that affect feasibility vs ikActiveDofs = solver.getActiveDofs() if self.costFunction is not None or self.feasibilityTest is not None: activeDofs = [i for i in range(len(qmin)) if qmin[i] != qmax[i]] nonIKDofs = [i for i in activeDofs if i not in ikActiveDofs] ikToActive = [activeDofs.index(i) for i in ikActiveDofs] else: activeDofs = ikActiveDofs nonIKDofs = [] ikToActive = range(len(activeDofs)) else: activeDofs = ikActiveDofs nonIKDofs = [] ikToActive = range(len(ikActiveDofs)) #sample random start point if params.startRandom: solver.sampleInitial() if len(nonIKDofs)>0: q = robot.getConfig() for i in nonIKDofs: q[i] = random.uniform(qmin[i],qmax[i]) robot.setConfig(q) if params.localMethod is not None or params.globalMethod is not None: #set up optProblem, an instance of optimize.Problem optProblem = optimize.Problem() Jactive = [[0.0]*len(activeDofs)]*len(solver.getResidual()) def ikConstraint(x): q = robot.getConfig() for d,v in zip(activeDofs,x): q[d] = v robot.setConfig(q) return solver.getResidual() def ikConstraintJac(x): q = robot.getConfig() for d,v in zip(activeDofs,x): q[d] = v robot.setConfig(q) Jikdofs = solver.getJacobian() for i in ikActiveDofs: for j in xrange(len(Jactive)): Jactive[j][ikToActive[i]] = Jikdofs[j][i] return Jactive def costFunc(x): q = robot.getConfig() for d,v in zip(activeDofs,x): q[d] = v return self.costFunction(q) def feasFunc(x): q = robot.getConfig() for d,v in zip(activeDofs,x): q[d] = v return self.feasibilityTest(q) optProblem.addEquality(ikConstraint,ikConstraintJac) if len(qmax) > 0: optProblem.setBounds([qmin[d] for d in activeDofs],[qmax[d] for d in activeDofs]) if self.costFunction is None: optProblem.setObjective(lambda x:0) else: optProblem.setObjective(costFunc) if self.feasibilityTest is not None: optProblem.setFeasibilityTest(feasFunc) #optProblem is now ready to use if params.globalMethod is not None: #do global optimization and return method = params.globalMethod numIters = params.numIters tol = params.tol if params.globalMethod == 'random-restart': #use the GlobalOptimize version of random restarts assert params.localMethod is not None,"Need a localMethod for random-restart to work" method = params.globalMethod + '.' + params.localMethod numIters = [params.numRestarts,params.numIters] optSolver = optimize.GlobalOptimizer(method) elif params.localMethod is not None: #do a sequential optimization method = [params.globalMethod,params.localMethod] #co-opt params.numRestarts for the number of outer iterations? numIters = [params.numRestarts,params.numIters] optSolver = optimize.GlobalOptimizer(method=method) q = robot.getConfig() x0 = [q[d] for d in activeDofs] optSolver.setSeed(x0) (succ,res) = optSolver.solve(optProblem,numIters=numIters,tol=tol) if succ: q = robot.getConfig() for d,v in zip(activeDofs,res): q[d] = v #check feasibility if desired if self.feasibilityTest is not None and not self.feasibilityTest(q): print "Result from global optimize isn't feasible" return None if max(abs(v) for v in solver.getResidual()) > params.tol: print "Result from global optimize doesn't satisfy tolerance. Residual",vectorops.norm(solver.getResidual()) return None #passed print "Global optimize succeeded! Cost",self.costFunction(q) return q else: print "Global optimize returned failure" return None #DONT DO THIS... much faster to do newton solves first, then local optimize. if not postOptimize and params.localMethod is not None: #random restart + local optimize optSolver = optimize.LocalOptimizer(method=params.localMethod) q = robot.getConfig() x0 = [q[d] for d in activeDofs] optSolver.setSeed(x0) best = None bestQuality = float('inf') for restart in xrange(params.numRestarts): if time.time() - t0 > params.timeout: return best res = optSolver.solve(optProblem,params.numIters,params.tol) if res[0]: q = robot.getConfig() for d,v in zip(activeDofs,res[1]): q[d] = v #check feasibility if desired if self.feasibilityTest is not None and not self.feasibilityTest(q): continue if self.costFunction is None: #feasible return q else: #optimize quality = self.costFunction(q) if quality < bestQuality: best = q bestQuality = quality #random restart solver.sampleInitial() q = robot.getConfig() if len(nonIKDofs)>0: for i in nonIKDofs: q[i] = random.uniform(qmin[i],qmax[i]) x0 = [q[d] for d in activeDofs] optSolver.setSeed(x0) else: #random-restart newton-raphson best = None bestQuality = float('inf') for restart in xrange(params.numRestarts): if time.time() - t0 > params.timeout: return best t0 = time.time() if solver.solve(params.numIters,params.tol)[0]: q = robot.getConfig() #check feasibility if desired t0 = time.time() if self.feasibilityTest is not None and not self.feasibilityTest(q): if len(nonIKDofs) > 0: u = float(restart+0.5)/params.numRestarts q = robot.getConfig() #perturbation sampling for i in nonIKDofs: delta = u*(qmax[i]-qmin[i])*0.5 q[i] = random.uniform(max(q[i]-delta,qmin[i]),min(q[i]+delta,qmax[i])) robot.setConfig(q) if not self.feasibilityTest(q): solver.sampleInitial() continue else: solver.sampleInitial() continue if self.costFunction is None: #feasible return q else: #optimize quality = self.costFunction(q) if quality < bestQuality: best = q bestQuality = quality #sample a new ik seed solver.sampleInitial() #post-optimize using local optimizer if postOptimize and best is not None and params.localMethod is not None: optSolver = optimize.LocalOptimizer(method=params.localMethod) x0 = [best[d] for d in activeDofs] optSolver.setSeed(x0) res = optSolver.solve(optProblem,params.numIters,params.tol) if res[0]: q = robot.getConfig() for d,v in zip(activeDofs,res[1]): q[d] = v #check feasibility if desired if self.feasibilityTest is not None and not self.feasibilityTest(q): pass elif self.costFunction is None: #feasible best = q else: #optimize quality = self.costFunction(q) if quality < bestQuality: #print "Optimization improvement",bestQuality,"->",quality best = q bestQuality = quality elif quality > bestQuality + 1e-2: print "Got worse solution by local optimizing?",bestQuality,"->",quality return best
rgroup.addFrame("marker (ground truth)",parent="marker link",relativeCoordinates=Tm0) for i,(obs,obs0) in enumerate(zip(observations,trueObservations)): rgroup.addFrame("obs"+str(i)+" (ground truth)",parent="camera (ground truth)",relativeCoordinates=obs0) rgroup.addFrame("obs"+str(i)+" (from camera)",parent="camera (ground truth)",relativeCoordinates=obs) vis.add("world",world) for i,q in enumerate(calibrationConfigs): vis.add("config"+str(i),q) app = lc.appearance().clone() app.setColor(0.5,0.5,0.5,0.1) vis.setAppearance("config"+str(i),app) vis.add("simulated coordinates",rgroup) vis.dialog() res = calibrate_robot_camera(robot,camera_link, calibrationConfigs, observations, [marker_link]*len(calibrationConfigs)) print print "Per-observation reconstruction error:",res[0]/numObs print "Estimated camera transform:",res[1] print " total error:",vectorops.norm(se3.error(res[1],Tc0)) print " rotation errors:",se3.error(res[1],Tc0)[:3] print " translation errors:",se3.error(res[1],Tc0)[3:] print "Estimated marker transform:",res[2][marker_link] print " error:",vectorops.norm(se3.error(res[2][marker_link],Tm0)) print " rotation errors:",se3.error(res[2][marker_link],Tm0)[:3] print " translation errors:",se3.error(res[2][marker_link],Tm0)[3:] vis.kill()
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)