def aoa_full(camera, room, debug): #lights=[(([-500, 0]),((-5, 0, 0),)),(([500, 0]),((5, 0, 0),)),(([0, -500]),((0, -5, 0),)),(([0, 500]), ((0, 5, 0),)),(([5, 0]),((0, 0, 0),)),] #lights=[((-1803.1,-1594.5),((-0.5,0.0,0.0),)),((1988.0,-1377.0),((0.0,-0.5,0.0),)),((1.8,110.2),((0.0,0.0,0.0),)),((-1448.0,1129.5),((0.0,0.5,0.0),)),((1306.5,1264.5),((0.5,0.0,0.0),)),] lights = get_light(); logger.debug('Raw lights information: {}'.format(lights)) # Some frequencies have multiple locations, need to pick one lights = resolve_aliased_frequncies(lights) # AoA calcualation requires at least 3 transmitters assert len(lights) >= 3, "AoA calcualation requires at least 3 transmitters" tries = 3 tries_rx_loc = numpy.empty([tries, 3]) tries_rx_rot = numpy.empty([tries, 3, 3]) tries_rx_err = numpy.empty([tries]) tries_method = ['YS_brute', 'static', 'scipy_basin'] tries_method = ['static'] #tries_method = ['YS_brute'] #tries_method = ['scipy_basin'] #tries_method = ['scipy_brute'] for i in xrange(tries): rx_location, rx_rotation, location_error = aoa(lights, camera.Zf, k_init_method=tries_method[i]) logger.info('location estimate = {}'.format(rx_location)) logger.info('location error = {}'.format(location_error)) tries_rx_loc[i] = rx_location tries_rx_rot[i] = rx_rotation tries_rx_err[i] = location_error if location_error > 1: if i == tries - 1: min_err_idx = numpy.argmin(tries_rx_err) rx_location = tries_rx_loc[min_err_idx] rx_rotation = tries_rx_rot[min_err_idx] location_error = tries_rx_err[min_err_idx] logger.info('Error ({}) too high, but max tries exceeded'.format(location_error)) logger.warn('Returning measurement with high error estimate') else: logger.info('Error ({}) too high, trying again'.format(location_error)) else: break ''' for k in xrange(len(lights)): aoa_lights = [lights[k]] r = range(len(lights)) r.pop(k) for i in r: aoa_lights.append(lights[i]) logger.info('aoa_lights = {}'.format(aoa_lights)) rx_location, rx_rotation, location_error = aoa(aoa_lights, camera.Zf) logger.info('location with brute k_init = {}'.format(rx_location)) logger.info('location error = {}'.format(location_error)) ''' return (rx_location, rx_rotation, location_error)
def aoa_full(file_name, camera, room, imag_proc, debug): frequencies = numpy.array(room.transmitters.keys()) logger.debug("Transmitter frequencies = {}".format(frequencies)) positions_of_lights, radii_of_lights, frequencies_of_lights, image_shape =\ imag_proc(file_name, 0, camera, debug) assert image_shape[0] > image_shape[1], "Processed image is not oriented correctly?" # Image is mirrored by the lens, need to reflect the image if not hasattr(room, 'origin'): raise TypeError('Room object {} does not have an origin'.format(room)) if room.origin == 'center': # Image origin is currently the top left but we want to center it center_point = tuple([p /2 for p in image_shape]) positions_of_lights[:,0] = center_point[0] - positions_of_lights[:,0] positions_of_lights[:,1] = center_point[1] - positions_of_lights[:,1] elif room.origin == 'south-east': positions_of_lights[:,1] = -positions_of_lights[:,1] else: raise NotImplementedError('Unknown origin type: {}'.format(room.origin)) logger.debug('Translated light center points: {}'.format( positions_of_lights), remove_newlines=True) # Drop frequencies that are unreasonably low to_del = [] for i in range(len(frequencies_of_lights)): if frequencies_of_lights[i] < 750: logger.debug("Deleting light with freq too low: {}".format(frequencies_of_lights[i])) to_del.append(i) positions_of_lights = numpy.delete(positions_of_lights, to_del, axis=0) radii_of_lights = numpy.delete(radii_of_lights, to_del) frequencies_of_lights = numpy.delete(frequencies_of_lights, to_del) # Convert the measured frequencies to the actual transmitted frequencies: actual_frequencies = [frequencies[(numpy.abs(frequencies - f)).argmin()] for f in frequencies_of_lights] logger.debug("Original frequencies: {}".format(frequencies_of_lights)) logger.debug(" Actual frequencies: {}".format(actual_frequencies)) del(frequencies_of_lights) # delete this so we don't accidentally use it if len(actual_frequencies) != len(numpy.unique(actual_frequencies)): logger.start_op('Removing duplicate transmitter entries') uniq_freq = numpy.unique(actual_frequencies) for freq in uniq_freq: matches = [] for i in xrange(len(actual_frequencies)): if freq == actual_frequencies[i]: matches.append((i, positions_of_lights[i], radii_of_lights[i])) if len(matches) > 1: idxs, centers, radii = zip(*matches) logger.debug('Duplicate frequency {} --'\ ' idxs {} centers {} radii {}'.format( freq, idxs, centers, radii)) logger.debug('pos = {}'.format(positions_of_lights)) positions_of_lights = numpy.delete(positions_of_lights, idxs, axis=0) logger.debug('pos = {}'.format(positions_of_lights)) logger.debug('rad = {}'.format(radii_of_lights)) radii_of_lights = numpy.delete(radii_of_lights, idxs) logger.debug('rad = {}'.format(radii_of_lights)) actual_frequencies = numpy.delete(actual_frequencies, idxs) best_idx = numpy.argmax(radii) logger.debug('pos = {}'.format(positions_of_lights)) logger.debug('centers[{}] = {}'.format(best_idx, centers[best_idx])) positions_of_lights = numpy.vstack((positions_of_lights, centers[best_idx])) logger.debug('pos = {}'.format(positions_of_lights)) radii_of_lights = numpy.append(radii_of_lights, radii[best_idx]) actual_frequencies = numpy.append(actual_frequencies, freq) logger.debug('After duplicate removal:') for pos, rad, freq in zip(positions_of_lights, radii_of_lights, actual_frequencies): logger.debug(' {} with radius {}\t= {} Hz'.format(pos, rad, freq)) logger.end_op() # Create pairs (light_position_on_image, transmitter_position) assert len(positions_of_lights) == len(actual_frequencies), "# of center points != # of frequencies?" lights = [ ( positions_of_lights[i], room.transmitters[actual_frequencies[i]] ) for i in xrange(len(positions_of_lights))] logger.debug('Raw lights information: {}'.format(lights)) # Some frequencies have multiple locations, need to pick one lights = resolve_aliased_frequncies(lights) # AoA calcualation requires at least 3 transmitters assert len(lights) >= 3, "AoA calcualation requires at least 3 transmitters" tries = 1 tries_rx_loc = numpy.empty([tries, 3]) tries_rx_rot = numpy.empty([tries, 3, 3]) tries_rx_err = numpy.empty([tries]) #tries_method = ['YS_brute', 'static', 'scipy_basin'] tries_method = ['static'] for i in xrange(tries): rx_location, rx_rotation, location_error = aoa(lights, camera.Zf, k_init_method=tries_method[i]) logger.info('location estimate = {}'.format(rx_location)) logger.info('location error = {}'.format(location_error)) tries_rx_loc[i] = rx_location tries_rx_rot[i] = rx_rotation tries_rx_err[i] = location_error if location_error > 1: if i == tries - 1: min_err_idx = numpy.argmin(tries_rx_err) rx_location = tries_rx_loc[min_err_idx] rx_rotation = tries_rx_rot[min_err_idx] location_error = tries_rx_err[min_err_idx] logger.info('Error ({}) too high, but max tries exceeded'.format(location_error)) logger.warn('Returning measurement with high error estimate') else: logger.info('Error ({}) too high, trying again'.format(location_error)) else: break ''' for k in xrange(len(lights)): aoa_lights = [lights[k]] r = range(len(lights)) r.pop(k) for i in r: aoa_lights.append(lights[i]) logger.info('aoa_lights = {}'.format(aoa_lights)) rx_location, rx_rotation, location_error = aoa(aoa_lights, camera.Zf) logger.info('location with brute k_init = {}'.format(rx_location)) logger.info('location error = {}'.format(location_error)) ''' return (rx_location, rx_rotation, location_error)
def aoa_full(file_name, camera, room, imag_proc, actual_location=None, k_val_method=None): frequencies = numpy.array(room.transmitters.keys()) positions_of_lights, radii_of_lights, frequencies_of_lights, image_shape =\ imag_proc(file_name, 0, camera) assert image_shape[0] > image_shape[1], "Processed image is not oriented correctly?" # Image is mirrored by the lens, need to reflect the image assert len(positions_of_lights) == len(radii_of_lights) assert len(radii_of_lights) == len(frequencies_of_lights) # Image origin is currently the top left but we want to center it, the AoA # processing assumes that (0, 0, 0) in the receiver coordinate system is at # the center of the lens center_point = tuple([p /2 for p in image_shape]) positions_of_lights[:,0] = center_point[0] - positions_of_lights[:,0] positions_of_lights[:,1] = center_point[1] - positions_of_lights[:,1] # 90 # positions_of_lights[:,0] = -(center_point[1] - positions_of_lights[:,1]) # positions_of_lights[:,1] = center_point[0] - positions_of_lights[:,0] # -90 # positions_of_lights[:,0] = center_point[1] - positions_of_lights[:,1] # positions_of_lights[:,1] = -(center_point[0] - positions_of_lights[:,0]) logger.debug2('Translated light center points: {}'.format( positions_of_lights), remove_newlines=True) last_f = None min_freq_diff = 10000 for f in sorted(room.transmitters): if last_f is None: last_f = f continue min_freq_diff = min(min_freq_diff, f-last_f) last_f = f del(last_f) # Lock frequencies actual_frequencies = [ cround(f, min_freq_diff) for f in frequencies_of_lights ] del(frequencies_of_lights) # delete this so we don't accidentally use it del(min_freq_diff) assert len(positions_of_lights) == len(radii_of_lights) assert len(radii_of_lights) == len(actual_frequencies) # Drop unknown frequencies to_del = [] for i in range(len(actual_frequencies)): if actual_frequencies[i] not in room.transmitters: logger.debug2("Deleting light with unknown freq: {}".format(actual_frequencies[i])) to_del.append(i) positions_of_lights = numpy.delete(positions_of_lights, to_del, axis=0) radii_of_lights = numpy.delete(radii_of_lights, to_del) actual_frequencies = numpy.delete(actual_frequencies, to_del) assert len(positions_of_lights) == len(radii_of_lights) assert len(radii_of_lights) == len(actual_frequencies) # Delete duplicate frequencies to_del = [] dups = {} for i in range(len(actual_frequencies)): if actual_frequencies[i] in dups: logger.debug("Deleting dup freq entry {}".format(actual_frequencies[i])) to_del.append(i) # Check if close by? l1 = positions_of_lights[i] l2 = positions_of_lights[dups[actual_frequencies[i]]] if (l1[0] - l2[0])**2 + (l1[0] - l2[0])**2 > 100**2: logger.debug("Remove orig too, far away") to_del.append(dups[actual_frequencies[i]]) else: logger.debug("keep orig") else: dups[actual_frequencies[i]] = i positions_of_lights = numpy.delete(positions_of_lights, to_del, axis=0) radii_of_lights = numpy.delete(radii_of_lights, to_del) actual_frequencies = numpy.delete(actual_frequencies, to_del) assert len(positions_of_lights) == len(radii_of_lights) assert len(radii_of_lights) == len(actual_frequencies) # if len(actual_frequencies) != len(numpy.unique(actual_frequencies)): # logger.start_op('Removing duplicate transmitter entries') # uniq_freq = numpy.unique(actual_frequencies) # for freq in uniq_freq: # matches = [] # for i in xrange(len(actual_frequencies)): # if freq == actual_frequencies[i]: # matches.append((i, positions_of_lights[i], radii_of_lights[i])) # if len(matches) > 1: # idxs, centers, radii = zip(*matches) # logger.debug('Duplicate frequency {} --'\ # ' idxs {} centers {} radii {}'.format( # freq, idxs, centers, radii)) # logger.debug('pos = {}'.format(positions_of_lights)) # positions_of_lights = numpy.delete(positions_of_lights, idxs, axis=0) # logger.debug('pos = {}'.format(positions_of_lights)) # logger.debug('rad = {}'.format(radii_of_lights)) # radii_of_lights = numpy.delete(radii_of_lights, idxs) # logger.debug('rad = {}'.format(radii_of_lights)) # actual_frequencies = numpy.delete(actual_frequencies, idxs) # best_idx = numpy.argmax(radii) # logger.debug('pos = {}'.format(positions_of_lights)) # logger.debug('centers[{}] = {}'.format(best_idx, centers[best_idx])) # positions_of_lights = numpy.vstack((positions_of_lights, centers[best_idx])) # logger.debug('pos = {}'.format(positions_of_lights)) # radii_of_lights = numpy.append(radii_of_lights, radii[best_idx]) # actual_frequencies = numpy.append(actual_frequencies, freq) # logger.debug('After duplicate removal:') # for pos, rad, freq in zip(positions_of_lights, radii_of_lights, actual_frequencies): # logger.debug(' {} with radius {}\t= {} Hz'.format(pos, rad, freq)) # logger.end_op() # Create pairs (light_position_on_image, transmitter_position) assert len(positions_of_lights) == len(actual_frequencies), "# of center points != # of frequencies?" lights = [ ( positions_of_lights[i], room.transmitters[actual_frequencies[i]] ) for i in xrange(len(positions_of_lights))] logger.debug('Raw lights information:') for i in range(len(lights)): logger.debug('\t{}: {}'.format(actual_frequencies[i], lights[i])) # Some frequencies have multiple locations, need to pick one # # lights = resolve_aliased_frequncies(lights) # AoA calcualation requires at least 3 transmitters assert len(lights) >= 3, "AoA calcualation requires at least 3 transmitters" tries = 1 tries_rx_loc = numpy.empty([tries, 3]) tries_rx_rot = numpy.empty([tries, 3, 3]) tries_rx_err = numpy.empty([tries]) #tries_method = ['YS_brute', 'static', 'scipy_basin'] if k_val_method: tries_method = [k_val_method] else: tries_method = ['static'] for i in xrange(tries): rx_location, rx_rotation, location_error =\ aoa( room, lights, camera.Zf, k_init_method=tries_method[i], actual_location=actual_location, ) logger.primary('location estimate = {}'.format(rx_location)) logger.primary('location error = {}'.format(location_error)) tries_rx_loc[i] = rx_location tries_rx_rot[i] = rx_rotation tries_rx_err[i] = location_error if location_error > 1: if i == tries - 1: min_err_idx = numpy.argmin(tries_rx_err) rx_location = tries_rx_loc[min_err_idx] rx_rotation = tries_rx_rot[min_err_idx] location_error = tries_rx_err[min_err_idx] logger.warn('Error ({}) too high, but max tries exceeded'.format(location_error)) logger.warn('Returning measurement with high error estimate') max_light_i = numpy.argmax(radii_of_lights) logger.warn('Prefer estimate of under max radii light:') logger.warn('Loc: {}'.format(room.transmitters[actual_frequencies[max_light_i]])) else: logger.info('Error ({}) too high, trying again'.format(location_error)) else: break ''' for k in xrange(len(lights)): aoa_lights = [lights[k]] r = range(len(lights)) r.pop(k) for i in r: aoa_lights.append(lights[i]) logger.info('aoa_lights = {}'.format(aoa_lights)) rx_location, rx_rotation, location_error = aoa(aoa_lights, camera.Zf) logger.info('location with brute k_init = {}'.format(rx_location)) logger.info('location error = {}'.format(location_error)) ''' return (rx_location, rx_rotation, location_error)
def aoa_full(file_name, camera, room, imag_proc, actual_location=None, k_val_method=None): frequencies = numpy.array(room.transmitters.keys()) positions_of_lights, radii_of_lights, frequencies_of_lights, image_shape =\ imag_proc(file_name, 0, camera) assert image_shape[0] > image_shape[ 1], "Processed image is not oriented correctly?" # Image is mirrored by the lens, need to reflect the image assert len(positions_of_lights) == len(radii_of_lights) assert len(radii_of_lights) == len(frequencies_of_lights) # Image origin is currently the top left but we want to center it, the AoA # processing assumes that (0, 0, 0) in the receiver coordinate system is at # the center of the lens center_point = tuple([p / 2 for p in image_shape]) positions_of_lights[:, 0] = center_point[0] - positions_of_lights[:, 0] positions_of_lights[:, 1] = center_point[1] - positions_of_lights[:, 1] # 90 # positions_of_lights[:,0] = -(center_point[1] - positions_of_lights[:,1]) # positions_of_lights[:,1] = center_point[0] - positions_of_lights[:,0] # -90 # positions_of_lights[:,0] = center_point[1] - positions_of_lights[:,1] # positions_of_lights[:,1] = -(center_point[0] - positions_of_lights[:,0]) logger.debug2( 'Translated light center points: {}'.format(positions_of_lights), remove_newlines=True) last_f = None min_freq_diff = 10000 for f in sorted(room.transmitters): if last_f is None: last_f = f continue min_freq_diff = min(min_freq_diff, f - last_f) last_f = f del (last_f) # Lock frequencies actual_frequencies = [ cround(f, min_freq_diff) for f in frequencies_of_lights ] del (frequencies_of_lights) # delete this so we don't accidentally use it del (min_freq_diff) assert len(positions_of_lights) == len(radii_of_lights) assert len(radii_of_lights) == len(actual_frequencies) # Drop unknown frequencies to_del = [] for i in range(len(actual_frequencies)): if actual_frequencies[i] not in room.transmitters: logger.debug2("Deleting light with unknown freq: {}".format( actual_frequencies[i])) to_del.append(i) positions_of_lights = numpy.delete(positions_of_lights, to_del, axis=0) radii_of_lights = numpy.delete(radii_of_lights, to_del) actual_frequencies = numpy.delete(actual_frequencies, to_del) assert len(positions_of_lights) == len(radii_of_lights) assert len(radii_of_lights) == len(actual_frequencies) # Delete duplicate frequencies to_del = [] dups = {} for i in range(len(actual_frequencies)): if actual_frequencies[i] in dups: logger.debug("Deleting dup freq entry {}".format( actual_frequencies[i])) to_del.append(i) # Check if close by? l1 = positions_of_lights[i] l2 = positions_of_lights[dups[actual_frequencies[i]]] if (l1[0] - l2[0])**2 + (l1[0] - l2[0])**2 > 100**2: logger.debug("Remove orig too, far away") to_del.append(dups[actual_frequencies[i]]) else: logger.debug("keep orig") else: dups[actual_frequencies[i]] = i positions_of_lights = numpy.delete(positions_of_lights, to_del, axis=0) radii_of_lights = numpy.delete(radii_of_lights, to_del) actual_frequencies = numpy.delete(actual_frequencies, to_del) assert len(positions_of_lights) == len(radii_of_lights) assert len(radii_of_lights) == len(actual_frequencies) # if len(actual_frequencies) != len(numpy.unique(actual_frequencies)): # logger.start_op('Removing duplicate transmitter entries') # uniq_freq = numpy.unique(actual_frequencies) # for freq in uniq_freq: # matches = [] # for i in xrange(len(actual_frequencies)): # if freq == actual_frequencies[i]: # matches.append((i, positions_of_lights[i], radii_of_lights[i])) # if len(matches) > 1: # idxs, centers, radii = zip(*matches) # logger.debug('Duplicate frequency {} --'\ # ' idxs {} centers {} radii {}'.format( # freq, idxs, centers, radii)) # logger.debug('pos = {}'.format(positions_of_lights)) # positions_of_lights = numpy.delete(positions_of_lights, idxs, axis=0) # logger.debug('pos = {}'.format(positions_of_lights)) # logger.debug('rad = {}'.format(radii_of_lights)) # radii_of_lights = numpy.delete(radii_of_lights, idxs) # logger.debug('rad = {}'.format(radii_of_lights)) # actual_frequencies = numpy.delete(actual_frequencies, idxs) # best_idx = numpy.argmax(radii) # logger.debug('pos = {}'.format(positions_of_lights)) # logger.debug('centers[{}] = {}'.format(best_idx, centers[best_idx])) # positions_of_lights = numpy.vstack((positions_of_lights, centers[best_idx])) # logger.debug('pos = {}'.format(positions_of_lights)) # radii_of_lights = numpy.append(radii_of_lights, radii[best_idx]) # actual_frequencies = numpy.append(actual_frequencies, freq) # logger.debug('After duplicate removal:') # for pos, rad, freq in zip(positions_of_lights, radii_of_lights, actual_frequencies): # logger.debug(' {} with radius {}\t= {} Hz'.format(pos, rad, freq)) # logger.end_op() # Create pairs (light_position_on_image, transmitter_position) assert len(positions_of_lights) == len( actual_frequencies), "# of center points != # of frequencies?" lights = [(positions_of_lights[i], room.transmitters[actual_frequencies[i]]) for i in xrange(len(positions_of_lights))] logger.debug('Raw lights information:') for i in range(len(lights)): logger.debug('\t{}: {}'.format(actual_frequencies[i], lights[i])) # Some frequencies have multiple locations, need to pick one # # lights = resolve_aliased_frequncies(lights) # AoA calcualation requires at least 3 transmitters assert len( lights) >= 3, "AoA calcualation requires at least 3 transmitters" tries = 1 tries_rx_loc = numpy.empty([tries, 3]) tries_rx_rot = numpy.empty([tries, 3, 3]) tries_rx_err = numpy.empty([tries]) #tries_method = ['YS_brute', 'static', 'scipy_basin'] if k_val_method: tries_method = [k_val_method] else: tries_method = ['static'] for i in xrange(tries): rx_location, rx_rotation, location_error =\ aoa( room, lights, camera.Zf, k_init_method=tries_method[i], actual_location=actual_location, ) logger.primary('location estimate = {}'.format(rx_location)) logger.primary('location error = {}'.format(location_error)) tries_rx_loc[i] = rx_location tries_rx_rot[i] = rx_rotation tries_rx_err[i] = location_error if location_error > 1: if i == tries - 1: min_err_idx = numpy.argmin(tries_rx_err) rx_location = tries_rx_loc[min_err_idx] rx_rotation = tries_rx_rot[min_err_idx] location_error = tries_rx_err[min_err_idx] logger.warn( 'Error ({}) too high, but max tries exceeded'.format( location_error)) logger.warn('Returning measurement with high error estimate') max_light_i = numpy.argmax(radii_of_lights) logger.warn('Prefer estimate of under max radii light:') logger.warn('Loc: {}'.format( room.transmitters[actual_frequencies[max_light_i]])) else: logger.info( 'Error ({}) too high, trying again'.format(location_error)) else: break ''' for k in xrange(len(lights)): aoa_lights = [lights[k]] r = range(len(lights)) r.pop(k) for i in r: aoa_lights.append(lights[i]) logger.info('aoa_lights = {}'.format(aoa_lights)) rx_location, rx_rotation, location_error = aoa(aoa_lights, camera.Zf) logger.info('location with brute k_init = {}'.format(rx_location)) logger.info('location error = {}'.format(location_error)) ''' return (rx_location, rx_rotation, location_error)
import pretty_logger from aoa import aoa Zf = 2271 logger = pretty_logger.get_logger() w = 10 l = 10 print 'x' positions_of_lights = [(-450,450),(-450,450),(-450,450)] transmitters = [(-w/2, l/2, 0),(w/2, l/2, 0),(-w/2, -l/2, 0)] #positions_of_lights lights = [ ( positions_of_lights[i], transmitters[i] ) for i in xrange(3)] logger.debug('Raw lights information: {}'.format(lights)) tries_method = ['static'] rx_location, rx_rotation, location_error = aoa(lights, Zf, k_init_method=tries_method) logger.info('location estimate = {}'.format(rx_location))