def __init__(self, _init_coverageProbMap, num_particles, sflist):
'''
Constructor
'''
# Initialize attributes
self.c_map = _init_coverageProbMap
self.num_particles = num_particles
self.numRevisit = 0
self.numlandmarks = 0
self.control = SMController()
self.localMaps = []
self.landmarks = {}
# Initialize particle dictionary
self.particles = {}
sf0 = sflist[0] # the location of the 1st sensorfly in the real world is used as a relative origin
for sf in sflist:
# Keep a local coverage map for each SensorFly
sensorflyOwnMap = GridMap(self.c_map.length, self.c_map.breadth,
self.c_map.celllength, self.c_map.cellbreadth)
self.localMaps.append(sensorflyOwnMap)
plist = []
pwts = []
for i in range(num_particles): #@UnusedVariable
# A list of particles for each SensorFly
p = Particle(sf, float(1)/float(num_particles), sensorflyOwnMap, sf0)
plist.append(p)
pwts.append(float(1)/float(num_particles))
self.particles[sf.name] = [plist, pwts]
class SugarMap(object):
'''
SugarMap location multi-agent coverage algorithm
'''
def __init__(self, _init_coverageProbMap, num_particles, sflist):
'''
Constructor
'''
# Initialize attributes
self.c_map = _init_coverageProbMap
self.num_particles = num_particles
self.numRevisit = 0
self.numlandmarks = 0
self.control = SMController()
self.localMaps = []
self.landmarks = {}
# Initialize particle dictionary
self.particles = {}
sf0 = sflist[0] # the location of the 1st sensorfly in the real world is used as a relative origin
for sf in sflist:
# Keep a local coverage map for each SensorFly
sensorflyOwnMap = GridMap(self.c_map.length, self.c_map.breadth,
self.c_map.celllength, self.c_map.cellbreadth)
self.localMaps.append(sensorflyOwnMap)
plist = []
pwts = []
for i in range(num_particles): #@UnusedVariable
# A list of particles for each SensorFly
p = Particle(sf, float(1)/float(num_particles), sensorflyOwnMap, sf0)
plist.append(p)
pwts.append(float(1)/float(num_particles))
self.particles[sf.name] = [plist, pwts]
def command(self, sflist):
'''
Command as per the coverage algorithm
'''
# Give the coverage command
self.control.command(sflist, self.particles, self.c_map)
def update(self, sflist, deltick):
'''
Update the SugarMap
'''
# Predict particle position
self.predict(sflist, deltick)
# Obtain signature and apply particle weight update
self.correct(sflist)
# Merge coverage maps from particles
self.mergeLocalMaps(sflist)
# Merge coverage maps from SensorFly's
self.mergeGlobalMaps(sflist)
# Re-sample particles
self.resample(sflist)
def predict(self, sflist, deltick):
'''
Predict the coverage map for each particle using odometry
'''
for sf in sflist:
plist = self.particles[sf.name][0]
# Predict next coverage grid using odometry
turn = sf.lastCommand.turn
velocity = sf.lastCommand.velocity
for p in plist:
# TODO: Add noise to the odometry according to a model
p.setMoveCommand(turn, velocity)
p.update(sf, deltick)
def correct(self, sflist):
'''
Correct the weight for each particle using RToF signatures
'''
# TODO: See if this function can be optimized
for sf in sflist:
sig = sf.locSignature
plist = self.particles[sf.name][0]
pwts = self.particles[sf.name][1]
isRevisit = False
if sf.name in self.landmarks:
landmarks = self.landmarks[sf.name]
else:
self.landmarks[sf.name] = []
landmarks = self.landmarks[sf.name]
# Check if it is a revisited landmark and update particle weights
if landmarks:
for lm in landmarks:
if (lm.isRevisit(sig)):
isRevisit = True
self.numRevisit += 1
self.updateParticleWtOnRevisit(plist, pwts, lm.hash)
# if it is not a revisit add new landmark
if not isRevisit:
lm = Landmark(sig)
self.numlandmarks += 1
landmarks.append(lm)
# For each particle for a SensorFly store the estimated locations
for p in plist:
p.landmarkDict[lm.hash] = [p.x, p.y]
def resample(self, sflist):
'''
Resample the particle distribution based on the weights
'''
for sf in sflist:
plist = self.particles[sf.name][0]
pwts = self.particles[sf.name][1]
n = len(pwts)
indices = []
C = [0.] + [sum(pwts[:i+1]) for i in range(n)]
u0, j = np.random.random(), 0
for u in [(u0+i)/n for i in range(n)]:
while u > C[j]:
j+=1
indices.append(j-1)
plist = [plist[idx] for idx in indices]
plist[-1].weight = 1.0/n
pwts = [pwts[idx] for idx in indices]
def mergeLocalMaps(self, sflist):
'''
Merge the coverage map of all particles to compute the local probabilistic coverage
map for each SensorFly
'''
for sf, lmap in zip(sflist, self.localMaps):
plist = self.particles[sf.name][0]
lmap.map[:] = 0.0
for p in plist:
lmap.map = lmap.map + (p.localMap.map * p.weight)
def mergeGlobalMaps(self, sflist):
'''
Merge the coverage map of all SensorFly nodes to compute global probabilistic coverage map
'''
self.c_map.map[:] = 1.0
for lmap in self.localMaps:
self.c_map.map *= 1 - lmap.map
self.c_map.map = 1 - self.c_map.map
for sf in sflist:
plist = self.particles[sf.name][0] # get particle lists of the current sensorfly
loc = self.control.locEstimate(plist) # determine sensorfly's location using particles' locations
self.c_map.markMap(loc[0],loc[1],self.c_map.vNode)
def updateParticleWtOnRevisit(self, plist, pwts, hash_value):
'''
Update the weight of the particles when a revisit is identified according to a specified function
'''
# Update the weights in 2 passes
# TODO: Check the weight update function
dlist = []
for p in plist:
[x,y] = p.landmarkDict[hash_value]
d = math.sqrt((p.x - x)**2 + (p.y - y)**2)
d = math.exp(-d**2 / 9) # similarity function
dlist.append(d)
sumD = sum(dlist)
# Update weights in inverse ratio of their real distance
for i,p in enumerate(plist):
d = dlist[i]/sumD
p.weight = d
pwts[i] = p.weight
# OPTIMIZATION FOR OPENCL - Add to mergeLocalMaps
#
# ctx = cl.create_some_context()
# queue = cl.CommandQueue(ctx)
# program = self.loadProgram("mergemap.cl", ctx)
# mf = cl.mem_flags
# #initialize client side (CPU) arrays
# a = lmap.map
# b = p.localMap.map
# #create OpenCL buffers
# a_buf = cl.Buffer(ctx, mf.READ_ONLY | mf.COPY_HOST_PTR, hostbuf=a)
# b_buf = cl.Buffer(ctx, mf.READ_ONLY | mf.COPY_HOST_PTR, hostbuf=b)
# dest_buf = cl.Buffer(ctx, mf.WRITE_ONLY, b.nbytes)
#
# program.mergemap(queue, a.shape, None, a_buf, b_buf, dest_buf)
# c = np.empty_like(a)
# cl.enqueue_read_buffer(queue, dest_buf, c).wait()
# lmap.map = c
# def loadProgram(self, filename, ctx):
# '''
# Read in the OpenCL source file as a string
# '''
# f = open(filename, 'r')
# fstr = "".join(f.readlines())
# #create the program
# program = cl.Program(ctx, fstr).build()
# return program
