def getDataList():
""" Detect and record a list of Walabot sensor targets. Stop recording
and return the data when enough triggers has occured (according to the
SENSITIVITY) with no detection of targets.
Returns:
dataList: A list of the yPosCm attribute of the detected
sensor targets
"""
while True:
wlbt.Trigger()
targets = wlbt.GetTrackerTargets()
if targets:
targets = [max(targets, key=distance)]
numOfFalseTriggers = 0
triggersToStop = ASSUMED_FRAME_RATE * SENSITIVITY
while numOfFalseTriggers < triggersToStop:
wlbt.Trigger()
newTargets = wlbt.GetTrackerTargets()
if newTargets:
targets.append(max(newTargets, key=distance))
numOfFalseTriggers = 0
else:
numOfFalseTriggers += 1
yList = [
t.yPosCm for t in targets if abs(t.yPosCm) > IGNORED_LENGTH
]
if yList:
return yList
def startAndCalibrateWalabot():
""" Start the Walabot and calibrate it.
"""
wlbt.StartCalibration()
print('- Calibrating...')
while wlbt.GetStatus()[0] == wlbt.STATUS_CALIBRATING:
wlbt.Trigger()
wlbt.Start()
print('- Calibration ended.\n- Ready!')
def calibrate():
""" Calibrate radar. """
radar.StartCalibration()
app_status, calibration_process = radar.GetStatus()
while app_status == radar.STATUS_CALIBRATING:
radar.Trigger()
app_status, calibration_process = radar.GetStatus()
return
def run():
print("Connected to Walabot")
WalabotAPI.SetProfile(WalabotAPI.PROF_TRACKER)
# Set scan arena
WalabotAPI.SetArenaR(15, 40, 10)
WalabotAPI.SetArenaPhi(-60, 60, 10)
WalabotAPI.SetArenaTheta(-30, 30, 10)
print("Arena set")
# Set image filter
WalabotAPI.SetDynamicImageFilter(WalabotAPI.FILTER_TYPE_MTI)
WalabotAPI.SetThreshold(35)
# Start scan
WalabotAPI.Start()
with HandStateMachine(buzzer_wav, delay_frames) as hand_sm:
t = time.time()
while True:
WalabotAPI.Trigger()
energy = WalabotAPI.GetImageEnergy() * 1000
hand_sm.state_in(
) if energy > energy_threshold else hand_sm.state_out()
if debug:
print('Energy: {:<10}Frame Rate: {}'.format(
energy, 1 / (time.time() - t)))
t = time.time()
def targets(self):
bot.Trigger()
targets = bot.GetSensorTargets() # get targets from walabot
positions = []
for target in targets: # loop through all targets found
angle = math.degrees(math.atan(
target.yPosCm /
target.zPosCm)) # find angle of target on horizontal axis
distance = (
target.yPosCm**2 +
target.zPosCm**2)**0.5 # find radial distance to Walabot
positions.append(
(angle, distance)) # add (angle, distance) to list
return positions # return found positions
def verifyWalabotIsConnected():
""" Check for Walabot connectivity. loop until detect a Walabot.
"""
while True:
try:
wlbt.ConnectAny()
except wlbt.WalabotError as err:
input("- Connect Walabot and press 'Enter'.")
else:
print('- Connection to Walabot established.')
return
def connect(self): # connect to Walabot
while True: # keep trying until successful
try:
bot.ConnectAny() # try to connect
except bot.WalabotError as e:
if e.code == 19: # error code 19 means Walabot is not connected with the usb cable
input("Connect Walabot and press enter"
) # ask user to connect Walabot
else:
print("Connected to Walabot") # successfully connected
return # exit the loop
def start(self): # begin calibration and prepare for scanning
bot.SetProfile(bot.PROF_SENSOR) # set Walabot properties
bot.SetArenaR(minDistance, maxDistance, 5)
bot.SetArenaTheta(-1, 1, 1)
bot.SetArenaPhi(minAngle, maxAngle, 5)
bot.SetThreshold(60)
bot.SetDynamicImageFilter(bot.FILTER_TYPE_NONE)
bot.Start() # start calibration
bot.StartCalibration()
print("Calibrating")
while bot.GetStatus(
)[0] == bot.STATUS_CALIBRATING: # wait for calibration to end
bot.Trigger() # calibration process Walabot
print(".", end="")
print(CLEARSCREEN + moveCursor(0, 1) + "Calibrated: " + Fore.CYAN +
Style.BRIGHT + "True") # done calibrating
print(Style.RESET_ALL + "Player: " + Fore.YELLOW + Style.BRIGHT +
str(player))
def get_image():
'''Get image from Walabot server'''
threshold = 1.0
active = True
while active:
wb.Trigger()
M, _, _, _, _ = wb.GetRawImageSlice()
image = np.array([val for phi in M for val in phi],
dtype=np.float32)
yield image
if os.name == 'nt':
if kbhit():
key = ord(getch())
else:
key = -1
else:
key = stdscr.getch()
if key != -1:
if key == ord('q'):
active = False
elif key == 224:
if os.name == 'nt':
key = ord(getch())
else:
key = stdscr.getch()
if key == 72:
threshold += 0.1
elif key == 73:
threshold += 1.0
elif key == 80:
threshold -= 0.1
elif key == 81:
threshold -= 1.0
def animate(i):
wlbt.Trigger() # trigger walabot
ener = wlbt.GetImageEnergy() # use image energy of walabot to detect breathing
# array of actual energy values
# stores the array of energy values to be used in averaging
global y
y = np.asarray(y)
y = np.roll(y,-1)
y = y.tolist() # shift the y values down by one
y[num-1] = -ener
# array of averaged values
global y_sm
y_sm = np.asarray(y_sm)
y_sm = np.roll(y_sm,-1)
y_sm = y_sm.tolist() # shift the averaged y values down by one row
en_smth = np.sum(np.insert(y[num-5:num],0,0)) / 5 # averages the current y value with the previous 5 values
y_sm[num-1] = en_smth
# better smoothing
global x_smooth
y_smooth = spline(x, y_sm, x_smooth)
#find bpm
bpm, maxX, maxY, minX, minY= breathingrate(y_smooth)
# autoscaling
if i%10 == 1:
y_max=max(y_smooth)
y_min=min(y_smooth)
global ax
ax.set_ylim(y_min,y_max)
#print (y_smooth)
#print ("Min ",y_min)p
# write to data
line.set_data(x_smooth,y_smooth) # write new y value averages to plot
f.write(str(datetime.now())+ ',' + str(ener)+'\n') # write every value to a new line, with a timestamp
return line,
def run():
print("Connected to Walabot")
WalabotAPI.SetProfile(WalabotAPI.PROF_TRACKER)
# Set scan arena
WalabotAPI.SetArenaR(15, 40, 10)
WalabotAPI.SetArenaPhi(-60, 60, 10)
WalabotAPI.SetArenaTheta(-30, 30, 10)
print("Arena set")
# Set image filter
WalabotAPI.SetDynamicImageFilter(WalabotAPI.FILTER_TYPE_MTI)
WalabotAPI.SetThreshold(35)
print("Start Scan")
# Start scan
WalabotAPI.Start()
print("Passed")
t = time.time()
while True:
print("Triggering")
WalabotAPI.Trigger()
print("2nd")
WalabotAPI.GetRawImageSlice()
energy = WalabotAPI.GetImageEnergy() * 1000
print("3rd")
if energy > energy_threshold:
state = True
else:
state = False
if debug:
print('Energy: {:<10}Frame Rate: {}'.format(
energy, 1 / (time.time() - t)))
t = time.time()
def setWalabotSettings():
""" Configure Walabot's profile, arena (r, theta, phi), threshold and
the image filter.
"""
wlbt.SetProfile(wlbt.PROF_TRACKER)
wlbt.SetArenaR(R_MIN, R_MAX, R_RES)
wlbt.SetArenaTheta(THETA_MIN, THETA_MAX, THETA_RES)
wlbt.SetArenaPhi(PHI_MIN, PHI_MAX, PHI_RES)
wlbt.SetThreshold(THRESHOLD)
wlbt.SetDynamicImageFilter(wlbt.FILTER_TYPE_NONE)
print('- Walabot Configured.')
def run():
print("Connected to Walabot")
WalabotAPI.SetProfile(WalabotAPI.PROF_TRACKER)
# Set scan arena
WalabotAPI.SetArenaR(15, 60, 10)
WalabotAPI.SetArenaPhi(-60, 60, 5)
WalabotAPI.SetArenaTheta(-1, 1, 1)
print("Arena set")
# Set image filter
WalabotAPI.SetDynamicImageFilter(WalabotAPI.FILTER_TYPE_MTI)
WalabotAPI.SetThreshold(35)
# Start scan
WalabotAPI.Start()
with HandStateMachine(hh_wav, delay_frames) as right_sm:
with HandStateMachine(snare_wav, delay_frames) as left_sm:
while True:
WalabotAPI.Trigger()
raster_image, x, y, _, _ = WalabotAPI.GetRawImageSlice()
col_sum = [
sum([raster_image[i][j] for i in range(x)])
for j in range(y)
]
left_sum = sum(col_sum[:y // 2])
right_sum = sum(col_sum[y // 2:])
if debug:
print_data = '{:<130} Left Energy: {:<10} Right Energy: {:<10}'.format(
str(col_sum), left_sum, right_sum)
print(print_data)
left_sm.state_in(
) if left_sum > energy_threshold else left_sm.state_out()
right_sm.state_in(
) if right_sum > energy_threshold else right_sm.state_out()
parser.add_argument('--save_plot_path', type=str,
help='radar plot movie file name',
default=os.path.join(common.PRJ_DIR, 'ground-truth-samples.mp4'))
parser.add_argument('--logging_level', type=str,
help='logging level, "info" or "debug"',
default='info')
parser.set_defaults(realtime_plot=False)
parser.set_defaults(save_plot=False)
args = parser.parse_args()
logging.basicConfig(filename=os.path.join(common.PRJ_DIR, LOG_FILE),
filemode='w',
format='%(asctime)s %(name)-12s %(levelname)-8s %(message)s',
level=logging.DEBUG if args.logging_level=='debug' else logging.INFO)
radar.Init()
# Configure radar database install location.
radar.SetSettingsFolder()
# Establish communication with radar.
try:
radar.ConnectAny()
except radar.WalabotError as err:
logger.error(f'Failed to connect to radar.\nerror code: {str(err.code)}')
exit(1)
api_version = radar.GetVersion()
logger.info(f'Walabot api version: {api_version}')
# Set Profile.
def run():
cap = VideoCapture(0)
# Select scan arena
# R Phi Theta
ARENA = [(40, 500, 4), (-60, 60, 5), (-15, 15, 5)]
# Init of Dataframe
dataset = pd.DataFrame()
# Star Walabot capture process
print("Initialize API")
wb.Init()
wb.Initialize()
# Check if a Walabot is connected
try:
wb.ConnectAny()
except wb.WalabotError as err:
print("Failed to connect to Walabot.\nerror code: " + str(err.code))
print(wb.GetExtendedError())
print(wb.GetErrorString())
sys.exit(1)
ver = wb.GetVersion()
print("Walabot API version: {}".format(ver))
print("Connected to Walabot")
wb.SetProfile(wb.PROF_SENSOR)
# Set scan arena
wb.SetArenaR(*ARENA[0])
wb.SetArenaPhi(*ARENA[1])
wb.SetArenaTheta(*ARENA[2])
print("Arena set")
# Set image filter
wb.SetDynamicImageFilter(wb.FILTER_TYPE_NONE)
# Start calibration
wb.Start()
wb.StartCalibration()
while wb.GetStatus()[0] == wb.STATUS_CALIBRATING:
wb.Trigger()
print("Calibration done!")
namevar = time.strftime("%Y_%m_%d_%H_%M_%S")
dim1, dim2 = wb.GetRawImageSlice()[1:3]
try:
pairs = wb.GetAntennaPairs()
j = 1
# print(len(wb.GetAntennaPairs()))
# fig = plt.figure()
# ax = fig.add_subplot(111, projection='3d')
while True:
# One iteration takes around 0.1 seconds
wb.Trigger()
ret, frame = cap.read()
try:
raw_image, size_X, size_Y, size_Z, power = wb.GetRawImage()
raw_imageSlice, size_phi, size_r, slice_depth, powerSlice = wb.GetRawImageSlice(
)
sample_dict = {
'timestamp': time.time_ns(),
# 'raw_signals': raw_signals,
'wlb/img': raw_image,
'wlb/slice': raw_imageSlice,
'wlb/X': size_X,
'wlb/Y': size_Y,
'wlb/Z': size_Z,
'wlb/power': power,
'wlb/Sphi': size_phi,
'wlb/Sr': size_r,
'wlb/Sdepth': slice_depth,
'wlb/Spower': powerSlice,
'cam/img': frame
}
# plotpointcloud(raw_image, ax)
# if j%10 == 0:
# plt.show()
cv2.imshow('frame', frame)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
dataset = dataset.append(sample_dict, ignore_index=True)
# print(sample_dict["timestamp"])
except:
print(sys.exc_info())
j = j + 1
if j % 1000 == 0:
print("Saving at j=" + str(j))
dataset.to_pickle("walabot_{}_{}.pkl.zip".format(
namevar, int(j / 1000)),
compression='zip')
print("Saved!")
dataset = pd.DataFrame()
except KeyboardInterrupt:
print('interrupted!')
finally:
dataset.iloc[-1000:].to_pickle("walabot_{}_{}.pkl.zip".format(
namevar,
int(j / 1000) + 1),
compression='zip')
cap.release()
destroyAllWindows()
wb.Stop()
wb.Disconnect()
print("Done!")
sys.exit(0)
t = TargetPoint(i, target)
if debug: print(t.toJson())
sbs.send_event(walabot_hub, t.toJson())
if __name__ == "__main__":
key_name = "RootManageSharedAccessKey" # SharedAccessKeyName from Azure portal
sbs = ServiceBusService(service_namespace=sn,
shared_access_key_name=key_name,
shared_access_key_value=key_value)
sbs.create_event_hub(walabot_hub)
wala.Init()
wala.SetSettingsFolder()
try:
wala.ConnectAny()
except WalabotError as err:
if err.message == wala.WALABOT_INSTRUMENT_NOT_FOUND:
print('Please connect your Walabot')
else:
print(err.message)
wala.SetProfile(wala.PROF_SENSOR)
wala.SetDynamicImageFilter(wala.FILTER_TYPE_MTI)
wala.Start()
# create a socket to find the local IP address
serviceType = "_walabot-rc._tcp.local."
serviceName = "%s.%s" % (localIp.replace(".", "-"), serviceType)
serviceInfo = ServiceInfo(serviceType, serviceName, address=socket.inet_aton("127.0.0.1"), port=80, weight=0, priority=0, properties=b"")
zeroconf = Zeroconf()
zeroconf.register_service(serviceInfo)
print("Registered %s" % (serviceName))
# begin the Walabot initialization
print("[initialize]")
bot.Init()
bot.Initialize()
# connect to any Walabot over USB
print("[connect]")
try:
bot.ConnectAny()
except bot.WalabotError as e:
print(e)
exit()
# Set the configuration to be a very small cone just above the Walabot device
print("[configure]")
from __future__ import print_function
from sys import platform
from os import system
import WalabotAPI as wlbt
wlbt.Init() # load the WalabotSDK to the Python wrapper
wlbt.SetSettingsFolder() # set the path to the essetial database files
wlbt.ConnectAny() # establishes communication with the Walabot
wlbt.SetProfile(
wlbt.PROF_SENSOR_NARROW) # set scan profile out of the possibilities
# JC: replace PROF_SENSOR to PROF_SENSOR_NARROW
# Sensor narrow: Lower-resolution images for a fast capture rate. Useful for tracking quick movement.
# thus for heart rate
wlbt.SetDynamicImageFilter(
wlbt.FILTER_TYPE_DERIVATIVE) # specify filter to use
#JC: replace FILTER_TYPE_MTI to FILTER_TYPE_DERIVATIVE
#Moving Target Identification (MTI) filter, the Derivative filter is available for the specific frequencies typical of breathing.
## variables definition
## Walabot_SetArenaR - input parameters
minInCm = 30
maxInCm = 150
resICm = 1
## Walabot_SetArenaTheta - input parameters
minIndegrees = -4
maxIndegrees = 4
resIndegrees = 2
## Walabot_SetArenaPhi - input parameters
minPhiInDegrees = -4
maxPhiInDegrees = 4
# Check to ensure datasession files don't already exist
if (os.path.exists('../data/raw/framedata_' + parse_results.datasession +
'.json') or
(os.path.exists('../data/raw/rbg_' + parse_results.datasession + '.mp4'))):
print('Data session files already exist, cannot overwrite')
exit()
# Walbot Initialization
# SetProfile is chosen from a number of options, see Walabot API
# SetThreshold defines the minimum reflected power to be imaged
# SetDynamicImageFilter is chosen from a number of options, see walabot API
THRESHOLD = 15
wala.Init()
wala.SetSettingsFolder()
wala.ConnectAny()
wala.SetProfile(wala.PROF_SENSOR)
wala.SetThreshold(THRESHOLD)
wala.SetDynamicImageFilter(wala.FILTER_TYPE_NONE)
# Walabot 'Arena' settings
# *_R values define spherical radial distance of imaging
# All res_* values determine angle in degrees between antenna
# Some Arena settings commented out for testing
min_R, max_R, res_R = 216, 457, 5
wala.SetArenaR(min_R, max_R, res_R)
min_Theta, max_Theta, res_Theta = -19, 19, 5
print("3rd")
if energy > energy_threshold:
state = True
else:
state = False
if debug:
print('Energy: {:<10}Frame Rate: {}'.format(
energy, 1 / (time.time() - t)))
t = time.time()
if __name__ == '__main__':
print("Initialize API")
WalabotAPI.Init()
while True:
WalabotAPI.Initialize()
# Check if a Walabot is connected
try:
WalabotAPI.ConnectAny()
run()
except WalabotAPI.WalabotError as err:
print('Failed to connect to Walabot. error code: {}'.format(
str(err.code)))
except Exception as err:
print(err)
finally:
print("Cleaning API")
WalabotAPI.Clean()
def predict(min_proba):
# Load classifier along with the label encoder.
with open(os.path.join(common.PRJ_DIR, common.SVM_MODEL), 'rb') as fp:
model = pickle.load(fp)
with open(os.path.join(common.PRJ_DIR, common.LABELS), 'rb') as fp:
le = pickle.load(fp)
# Calculate size of radar image data array used for training.
train_size_z = int((common.R_MAX - common.R_MIN) / common.R_RES) + 1
train_size_y = int((common.PHI_MAX - common.PHI_MIN) / common.PHI_RES) + 1
train_size_x = int(
(common.THETA_MAX - common.THETA_MIN) / common.THETA_RES) + 1
logger.debug(f'train_size: {train_size_x}, {train_size_y}, {train_size_z}')
try:
while True:
# Scan according to profile and record targets.
radar.Trigger()
# Retrieve any targets from the last recording.
targets = radar.GetSensorTargets()
if not targets:
continue
# Retrieve the last completed triggered recording
raw_image, size_x, size_y, size_z, _ = radar.GetRawImage()
raw_image_np = np.array(raw_image, dtype=np.float32)
for t, target in enumerate(targets):
logger.info('**********')
logger.info(
'Target #{}:\nx: {}\ny: {}\nz: {}\namplitude: {}\n'.format(
t + 1, target.xPosCm, target.yPosCm, target.zPosCm,
target.amplitude))
i, j, k = common.calculate_matrix_indices(
target.xPosCm, target.yPosCm, target.zPosCm, size_x,
size_y, size_z)
# projection_yz is the 2D projection of target in y-z plane.
projection_yz = raw_image_np[i, :, :]
# projection_xz is the 2D projection of target in x-z plane.
projection_xz = raw_image_np[:, j, :]
# projection_xy is 2D projection of target signal in x-y plane.
projection_xy = raw_image_np[:, :, k]
proj_zoom = calc_proj_zoom(train_size_x, train_size_y,
train_size_z, size_x, size_y,
size_z)
observation = common.process_samples(
[(projection_xz, projection_yz, projection_xy)],
proj_mask=PROJ_MASK,
proj_zoom=proj_zoom,
scale=True)
# Make a prediction.
name, prob = classifier(observation, model, le, min_proba)
logger.info(f'Detected {name} with probability {prob}')
logger.info('**********')
except KeyboardInterrupt:
pass
finally:
# Stop and Disconnect.
radar.Stop()
radar.Disconnect()
radar.Clean()
logger.info('Successful radar shutdown.')
return
ani = animation.FuncAnimation(fig,
func=plot_update,
frames=get_image,
repeat=False,
interval=0,
blit=True)
try:
plt.show()
except:
pass
if __name__ == '__main__':
# Star Walabot capture process
print("Initialize API")
wb.Init()
wb.Initialize()
# Check if a Walabot is connected
try:
wb.ConnectAny()
except wb.WalabotError as err:
print("Failed to connect to Walabot.\nerror code: " + str(err.code))
sys.exit(1)
ver = wb.GetVersion()
print("Walabot API version: {}".format(ver))
print("Connected to Walabot")
wb.SetProfile(wb.PROF_TRACKER)
PHI_MIN, PHI_MAX, PHI_RES = -10, 10, 2 # SetArenaPhi parametes
THRESHOLD = 15 # SetThreshold parametes
MAX_Y_VALUE = R_MAX * cos(radians(THETA_MAX)) * sin(radians(PHI_MAX))
SENSITIVITY = 0.25 # amount of seconds to wait after a move has been detected
TENDENCY_LOWER_BOUND = 0.1 # tendency below that won't count as entrance/exit
IGNORED_LENGTH = 3 # len in cm to ignore targets in center of arena
ASSUMED_FRAME_RATE = 10
# TODO: Need to be configured to real server's ip and port
SERVER_ADDRESS = "127.0.0.1"
SERVER_PORT = 9999
# TODO: Need to be configured to real room's name
ROOM_NAME = "yellow"
# TODO: Need to be configured to real room's max people.
MAX_PEOPLE = 6
wlbt.Init()
wlbt.SetSettingsFolder()
def getNumOfPeopleInside():
""" Gets the current number of people in the room as input and returns it.
Validate that the number is valid.
Returns:
num Number of people in the room that got as input
"""
num = input('- Enter current number of people in the room: ')
if (not num.isdigit()) or (int(num) < 0):
print('- Invalid input, try again.')
return getNumOfPeopleInside()
return int(num)
try:
input = raw_input
except NameError:
pass
R_MIN, R_MAX, R_RES = 10, 60, 2 # SetArenaR parameters
THETA_MIN, THETA_MAX, THETA_RES = -10, 10, 10 # SetArenaTheta parameters
PHI_MIN, PHI_MAX, PHI_RES = -10, 10, 2 # SetArenaPhi parametes,
THRESHOLD = 15 # SetThreshold parametes
MAX_Y_VALUE = R_MAX * cos(radians(THETA_MAX)) * sin(radians(PHI_MAX))
SENSITIVITY = 0.25 # amount of seconds to wait after a move has been detected
TENDENCY_LOWER_BOUND = 0.1 # tendency below that won't count as entrance/exit
IGNORED_LENGTH = 3 # len in cm to ignore targets in center of arena
LIFX_TOKEN = '<Replace-with-LIFX-token-generated>'
wlbt.Init(
"C:\Program Files\Walabot\WalabotSDK\\bin\Win32\WalabotAPIWindows.dll")
wlbt.SetSettingsFolder()
def getNumOfPeopleInside():
""" Gets the current number of people in the room as input and returns it.
Validate that the number is valid.
Returns:
num Number of people in the room that got as input
"""
num = input('- Enter current number of people in the room: ')
if (not num.isdigit()) or (int(num) < 0):
print('- Invalid input, try again.')
return getNumOfPeopleInside()
return int(num)
def plot_and_capture_data(num_samples, realtime_plot, save_plot, save_plot_path, desired_labels):
def pol_2_cart_deg(a, r):
''' Convert polar coordinates, in degrees, to cartesian. '''
a_rad = np.deg2rad(a)
return (r * np.sin(a_rad), r * np.cos(a_rad))
def gen_pos_map():
''' Create position coordinates map for plotting. '''
arr_r = list(range(common.R_MIN, common.R_MAX, common.R_RES)) + [common.R_MAX]
arr_t = list(range(common.THETA_MIN, common.THETA_MAX, common.THETA_RES)) + [common.THETA_MAX]
arr_p = list(range(common.PHI_MIN, common.PHI_MAX, common.PHI_RES)) + [common.PHI_MAX]
# Format of pmap_xz is [[list of x],[list of z],[list of dot size]].
# Used to plot points on the XZ plane.
pmap_xz = np.array([list(pol_2_cart_deg(p, ra)) + [ra * 0.75] for ra in arr_r for p in arr_p]).T
# Format of pmap_yz is [[list of y],[list of z],[list of dot size]].
# Used to plot points on the YZ plane.
pmap_yz = np.array([list(pol_2_cart_deg(t, ra)) + [ra * 0.75] for ra in arr_r for t in arr_t]).T
return pmap_yz, pmap_xz
def init_position_markers(ax):
"""Initialize position markers and annotations."""
target_pt, = ax.plot(0, 0, 'ro', zorder=2)
target_ant = ax.annotate('target', xy=(0,0), color='red', zorder=2)
centroid_pt, = ax.plot(0, 0, 'go', zorder=3)
centroid_ant = ax.annotate('', xy=(0,0), color='green', zorder=3)
return (target_pt, target_ant, centroid_pt, centroid_ant)
def init_axis(ax, title, xlabel, ylabel):
"""Initialize axis labels."""
face_color = ScalarMappable(cmap='coolwarm').to_rgba(0)
ax.set_title(title)
ax.set_facecolor(face_color)
ax.set_xlabel(xlabel)
ax.set_ylabel(ylabel)
def update_plot(data):
'''Update plot for animation.'''
projections, name, target_position, centroid_position = data
projection_xz, projection_yz, projection_xy = projections
target_x, target_y, target_z = target_position
centroid_x, centroid_y = centroid_position
# Update target position and annotation from radar on plots.
target_ant_xz.set_position(xy=(target_x, target_z))
target_pt_xz.set_data(target_x, target_z)
target_ant_yz.set_position(xy=(target_y, target_z))
target_pt_yz.set_data(target_y, target_z)
target_ant_xy.set_position(xy=(target_x, target_y))
target_pt_xy.set_data(target_x, target_y)
# Update name and postion annotations of centroid from camera on plots
centroid_ant_xz.set_text(s=name)
centroid_ant_xz.set_position(xy=(centroid_x, target_z))
centroid_pt_xz.set_data(centroid_x, target_z)
centroid_ant_yz.set_text(s=name)
centroid_ant_yz.set_position(xy=(centroid_y, target_z))
centroid_pt_yz.set_data(centroid_y, target_z)
centroid_ant_xy.set_text(s=name)
centroid_ant_xy.set_position(xy=(centroid_x, centroid_y))
centroid_pt_xy.set_data(centroid_x, centroid_y)
# Update image colors according to return signal strength on plots.
sm = ScalarMappable(cmap='coolwarm')
signal_pts_xz.set_color(sm.to_rgba(projection_xz.T.flatten()))
sm = ScalarMappable(cmap='coolwarm')
signal_pts_yz.set_color(sm.to_rgba(projection_yz.T.flatten()))
# Scale xy image data relative to target distance.
signal_pts_xy.set_extent([v*target_z/(zmax-zmin) for v in [xmin,xmax,ymin,ymax]])
# Rotate xy image if radar horizontal since x and y axis are rotated 90 deg CCW.
if RADAR_HORIZONTAL:
projection_xy = np.rot90(projection_xy)
sm = ScalarMappable(cmap='coolwarm')
signal_pts_xy.set_data(sm.to_rgba(projection_xy))
return (signal_pts_xz, target_ant_xz, target_pt_xz, centroid_ant_xz, centroid_pt_xz,
signal_pts_yz, target_ant_yz, target_pt_yz, centroid_ant_yz, centroid_pt_yz,
signal_pts_xy, target_ant_xy, target_pt_xy, centroid_ant_xy, centroid_pt_xy)
if realtime_plot or save_plot:
if save_plot:
# Set up formatting for movie files.
Writer = animation.writers['ffmpeg']
writer = Writer(fps=15, metadata=dict(artist='lindo'), bitrate=1800)
# Get position maps.
pmap_xz, pmap_yz = gen_pos_map()
# Initial scan.
radar.Trigger()
raw_image, _, _, _, _ = radar.GetRawImage()
raw_image_np = np.array(raw_image, dtype=np.float32)
# projection_yz is the 2D projection of target in y-z plane.
# Transpose to match ordering of position map.
projection_yz = raw_image_np[0,:,:].transpose().flatten()
# projection_xz is the 2D projection of target in x-z plane.
projection_xz = raw_image_np[:,0,:].transpose().flatten()
#fig, (ax1, ax2, ax3) = plt.subplots(1, 3)
fig = plt.figure()
fig.suptitle('Target Return Signal, Target Position, Object Position and ID')
gs = fig.add_gridspec(2, 2)
ax1 = fig.add_subplot(gs[0,0])
ax2 = fig.add_subplot(gs[0,1])
ax3 = fig.add_subplot(gs[1,:])
# Setup x-z plane plot.
# Radar target return signal strength and camera centroid in x-z plane.
init_axis(ax1, 'X-Z Plane', 'X (cm)', 'Z (cm)')
signal_pts_xz = ax1.scatter(pmap_xz[0], pmap_xz[1], s=pmap_xz[2],
c=projection_xz, cmap='coolwarm', zorder=1)
(target_pt_xz, target_ant_xz, centroid_pt_xz,
centroid_ant_xz) = init_position_markers(ax1)
# Setup y-z plane plot.
# Radar target return signal strength and camera centroid in y-z plane.
init_axis(ax2, 'Y-Z Plane', 'Y (cm)', 'Z (cm)')
signal_pts_yz = ax2.scatter(pmap_yz[0], pmap_yz[1], s=pmap_yz[2],
c=projection_yz, cmap='coolwarm', zorder=1)
(target_pt_yz, target_ant_yz, centroid_pt_yz,
centroid_ant_yz) = init_position_markers(ax2)
# Setup x-y plane plot.
# Radar target return signal strength and camera centroid in x-z plane.
# NB: When radar is placed horizontally, a rotated image will be shown.
init_axis(ax3, 'X-Y Plane', 'X (cm)', 'Y (cm)')
# Calculate axis range to set axis limits and plot extent.
# Plot extent will change as a function of target distance.
xmax = np.amax(pmap_xz[0]).astype(np.int)
xmin = np.amin(pmap_xz[0]).astype(np.int)
ymax = np.amax(pmap_yz[0]).astype(np.int)
ymin = np.amin(pmap_yz[0]).astype(np.int)
zmax = np.amax(pmap_yz[1]).astype(np.int)
zmin = np.amin(pmap_yz[1]).astype(np.int)
ax3.set_xlim(xmax, xmin)
ax3.set_ylim(ymax, ymin)
init_img = np.zeros((xmax-xmin, ymax-ymin))
signal_pts_xy = ax3.imshow(init_img, cmap='coolwarm',
extent=[xmin,xmax,ymin,ymax], zorder=1)
(target_pt_xy, target_ant_xy, centroid_pt_xy,
centroid_ant_xy) = init_position_markers(ax3)
# Initialize ground truth data.
samples = []
labels = []
with grpc.insecure_channel(DETECT_SERVER_IP) as channel:
stub = detection_server_pb2_grpc.DetectionServerStub(channel)
res = get_camera_resolution(stub)
width, height = res.width, res.height
logger.info(f'camera resolution: {width, height}')
res = get_camera_intrinsic_parameters(stub)
fx, fy, cx, cy = res.fx, res.fy, res.cx, res.cy
logger.debug(f'camera intrinsics fx: {fx} fy:{fy} cx:{cx} cy:{cy}')
# Calculate camera field (aka angle) of view from intrinsics.
fov_hor = 2 * np.arctan(width / (2 * fx)) * 180.0 / np.pi
fov_ver = 2 * np.arctan(height / (2 * fy)) * 180.0 / np.pi
logger.info(f'camera hor fov: {fov_hor:.1f} (deg) ver fov: {fov_ver:.1f} (deg)')
def get_samples():
active = True
sample_num = 1
while active:
# Scan according to profile and record targets (if any).
radar.Trigger()
# Get object detection results from server (if any).
detected_objects = get_detected_objects(stub, desired_labels)
if not detected_objects:
continue
# Retrieve any targets from the last recording.
#targets = radar.GetTrackerTargets()
targets = radar.GetSensorTargets()
if not targets:
continue
# raw_image ordering: (theta, phi, r)
raw_image, size_x, size_y, size_z, _ = radar.GetRawImage()
raw_image_np = np.array(raw_image, dtype=np.float32)
logger.debug(f'Raw image np shape: {raw_image_np.shape}')
#targets = get_derived_targets(raw_image_np, size_x, size_y, size_z)
logger.info(f'Sample number {sample_num} of {num_samples}'.center(60, '-'))
# Find the detected object closest to each radar target.
for t, target in enumerate(targets):
logger.info(f'Target #{t + 1}:')
logger.debug('\nx: {}\ny: {}\nz: {}\namplitude: {}\n'.format(
target.xPosCm, target.yPosCm, target.zPosCm, target.amplitude))
i, j, k = common.calculate_matrix_indices(
target.xPosCm, target.yPosCm, target.zPosCm,
size_x, size_y, size_z)
logger.debug(f'i: {i}, j: {j}, k: {k}')
# Init distance between radar and camera target as a % of radar target depth.
# This is used as a threshold to declare correspondence.
current_distance = DETECTION_THRESHOLD_PERCENT * target.zPosCm
#current_distance = MAX_TARGET_OBJECT_DISTANCE
logger.debug(f'Initial threshold: {current_distance:.1f} (cm)')
target_object_close = False
for obj in detected_objects:
if obj.score < MIN_DETECTED_OBJECT_SCORE:
logger.debug(f'Object ({obj.label}) score ({obj.score:.1f}) too low...skipping.')
continue
# Convert position of detected object's centroid to radar coordinate system.
centroid_camera = (width*obj.centroid.x, height*obj.centroid.y)
logger.debug(f'Centroid camera: {centroid_camera}')
centroid_radar = convert_coordinates(centroid_camera,
target.zPosCm, fx, fy, cx, cy)
logger.debug(f'Centroid radar: {centroid_radar}')
# Calculate distance between detected object and radar target.
distance = compute_distance((target.xPosCm, target.yPosCm), centroid_radar)
logger.debug(f'Distance: {distance}')
# Find the detected object closest to the radar target.
if distance < current_distance:
target_object_close = True
current_distance = distance
current_score = obj.score
target_name = obj.label
target_position = target.xPosCm, target.yPosCm, target.zPosCm
centroid_position = centroid_radar
# Calculate 3D to 2D projections of target return signal.
# Signal in raw_image_np with shape (size_x, size_y, size_z).
# axis 1 and 2 of the matrix (j, k) contain the projections
# represented in angle phi and distance r, respectively.
# These are 2D projections the y-z plane.
# axis 0 and 2 of the matrix (i, k) contain the projections
# represented in angle theta and distance r, respectively.
# These are 2D projections the x-z plane.
#
# projection_yz is the 2D projection of target in y-z plane.
projection_yz = raw_image_np[i,:,:]
logger.debug(f'Projection_yz shape: {projection_yz.shape}')
# projection_xz is the 2D projection of target in x-z plane.
projection_xz = raw_image_np[:,j,:]
logger.debug(f'Projection_xz shape: {projection_xz.shape}')
# projection_xy is 2D projection of target signal in x-y plane.
projection_xy = raw_image_np[:,:,k]
logger.debug(f'Projection_xy shape: {projection_xy.shape}')
else:
msg = (
f'Found "{obj.label}" with score {obj.score:.1f} at {distance:.1f} (cm)'
f' too far from target at z {target.zPosCm:.1f} (cm)...skipping.'
)
logger.info(msg)
if target_object_close:
msg = (
f'Found "{target_name}" with score {current_score:.1f} at {distance:.1f} (cm)'
f' from target at z {target.zPosCm:.1f} (cm)...storing.'
)
logger.info(msg)
yield ((projection_xz, projection_yz, projection_xy),
target_name, target_position, centroid_position)
logger.info('-'*60+'\n')
if sample_num < num_samples:
sample_num += 1
else:
active = False
if realtime_plot:
print('\n**** Close plot window to continue. ****\n')
if realtime_plot or save_plot:
# Animate but do not save data.
ani = animation.FuncAnimation(fig, update_plot, frames=get_samples,
repeat=False, interval=100, blit=True)
try:
if realtime_plot:
plt.show()
elif save_plot:
ani.save(save_plot_path, writer=writer)
except Exception as e:
print(f'Unhandled animation exception: {e}')
pass
else:
# Save data but do not animate.
for data in get_samples():
projections, target_name, _, _ = data
samples.append(projections)
labels.append(target_name)
return samples, labels
def main():
radar.Init()
# Configure Walabot database install location.
radar.SetSettingsFolder()
# Establish communication with walabot.
try:
radar.ConnectAny()
except radar.WalabotError as err:
print(f'Failed to connect to Walabot.\nerror code: {str(err.code)}')
exit(1)
# Set radar scan profile.
radar.SetProfile(common.RADAR_PROFILE)
# Set scan arena in polar coords
radar.SetArenaR(common.R_MIN, common.R_MAX, common.R_RES)
radar.SetArenaPhi(common.PHI_MIN, common.PHI_MAX, common.PHI_RES)
radar.SetArenaTheta(common.THETA_MIN, common.THETA_MAX, common.THETA_RES)
# Threshold
radar.SetThreshold(RADAR_THRESHOLD)
# radar filtering
filter_type = radar.FILTER_TYPE_MTI if MTI else radar.FILTER_TYPE_NONE
radar.SetDynamicImageFilter(filter_type)
# Start the system in preparation for scanning.
radar.Start()
# Calibrate scanning to ignore or reduce the signals if not in MTI mode.
if not MTI:
common.calibrate()
try:
while True:
# Scan according to profile and record targets.
radar.Trigger()
# Retrieve any targets from the last recording.
targets = radar.GetSensorTargets()
if not targets:
continue
# Retrieve the last completed triggered recording
raw_image, size_x, size_y, size_z, _ = radar.GetRawImage()
raw_image_np = np.array(raw_image, dtype=np.float32)
for t, target in enumerate(targets):
print(
'Target #{}:\nx: {}\ny: {}\nz: {}\namplitude: {}\n'.format(
t + 1, target.xPosCm, target.yPosCm, target.zPosCm,
target.amplitude))
i, j, k = common.calculate_matrix_indices(
target.xPosCm, target.yPosCm, target.zPosCm, size_x,
size_y, size_z)
# projection_yz is the 2D projection of target in y-z plane.
projection_yz = raw_image_np[i, :, :]
# projection_xz is the 2D projection of target in x-z plane.
projection_xz = raw_image_np[:, j, :]
# projection_xy is 2D projection of target signal in x-y plane.
projection_xy = raw_image_np[:, :, k]
observation = common.process_samples(
[(projection_xy, projection_yz, projection_xz)],
proj_mask=PROJ_MASK)
# Make a prediction.
name, prob = classifier(observation)
if name == 'person':
color_name = colored(name, 'green')
elif name == 'dog':
color_name = colored(name, 'yellow')
elif name == 'cat':
color_name = colored(name, 'blue')
else:
color_name = colored(name, 'red')
print(f'Detected {color_name} with probability {prob}\n')
except KeyboardInterrupt:
pass
finally:
# Stop and Disconnect.
radar.Stop()
radar.Disconnect()
radar.Clean()
print('Successful termination.')
def get_samples():
active = True
sample_num = 1
while active:
# Scan according to profile and record targets (if any).
radar.Trigger()
# Get object detection results from server (if any).
detected_objects = get_detected_objects(stub, desired_labels)
if not detected_objects:
continue
# Retrieve any targets from the last recording.
#targets = radar.GetTrackerTargets()
targets = radar.GetSensorTargets()
if not targets:
continue
# raw_image ordering: (theta, phi, r)
raw_image, size_x, size_y, size_z, _ = radar.GetRawImage()
raw_image_np = np.array(raw_image, dtype=np.float32)
logger.debug(f'Raw image np shape: {raw_image_np.shape}')
#targets = get_derived_targets(raw_image_np, size_x, size_y, size_z)
logger.info(f'Sample number {sample_num} of {num_samples}'.center(60, '-'))
# Find the detected object closest to each radar target.
for t, target in enumerate(targets):
logger.info(f'Target #{t + 1}:')
logger.debug('\nx: {}\ny: {}\nz: {}\namplitude: {}\n'.format(
target.xPosCm, target.yPosCm, target.zPosCm, target.amplitude))
i, j, k = common.calculate_matrix_indices(
target.xPosCm, target.yPosCm, target.zPosCm,
size_x, size_y, size_z)
logger.debug(f'i: {i}, j: {j}, k: {k}')
# Init distance between radar and camera target as a % of radar target depth.
# This is used as a threshold to declare correspondence.
current_distance = DETECTION_THRESHOLD_PERCENT * target.zPosCm
#current_distance = MAX_TARGET_OBJECT_DISTANCE
logger.debug(f'Initial threshold: {current_distance:.1f} (cm)')
target_object_close = False
for obj in detected_objects:
if obj.score < MIN_DETECTED_OBJECT_SCORE:
logger.debug(f'Object ({obj.label}) score ({obj.score:.1f}) too low...skipping.')
continue
# Convert position of detected object's centroid to radar coordinate system.
centroid_camera = (width*obj.centroid.x, height*obj.centroid.y)
logger.debug(f'Centroid camera: {centroid_camera}')
centroid_radar = convert_coordinates(centroid_camera,
target.zPosCm, fx, fy, cx, cy)
logger.debug(f'Centroid radar: {centroid_radar}')
# Calculate distance between detected object and radar target.
distance = compute_distance((target.xPosCm, target.yPosCm), centroid_radar)
logger.debug(f'Distance: {distance}')
# Find the detected object closest to the radar target.
if distance < current_distance:
target_object_close = True
current_distance = distance
current_score = obj.score
target_name = obj.label
target_position = target.xPosCm, target.yPosCm, target.zPosCm
centroid_position = centroid_radar
# Calculate 3D to 2D projections of target return signal.
# Signal in raw_image_np with shape (size_x, size_y, size_z).
# axis 1 and 2 of the matrix (j, k) contain the projections
# represented in angle phi and distance r, respectively.
# These are 2D projections the y-z plane.
# axis 0 and 2 of the matrix (i, k) contain the projections
# represented in angle theta and distance r, respectively.
# These are 2D projections the x-z plane.
#
# projection_yz is the 2D projection of target in y-z plane.
projection_yz = raw_image_np[i,:,:]
logger.debug(f'Projection_yz shape: {projection_yz.shape}')
# projection_xz is the 2D projection of target in x-z plane.
projection_xz = raw_image_np[:,j,:]
logger.debug(f'Projection_xz shape: {projection_xz.shape}')
# projection_xy is 2D projection of target signal in x-y plane.
projection_xy = raw_image_np[:,:,k]
logger.debug(f'Projection_xy shape: {projection_xy.shape}')
else:
msg = (
f'Found "{obj.label}" with score {obj.score:.1f} at {distance:.1f} (cm)'
f' too far from target at z {target.zPosCm:.1f} (cm)...skipping.'
)
logger.info(msg)
if target_object_close:
msg = (
f'Found "{target_name}" with score {current_score:.1f} at {distance:.1f} (cm)'
f' from target at z {target.zPosCm:.1f} (cm)...storing.'
)
logger.info(msg)
yield ((projection_xz, projection_yz, projection_xy),
target_name, target_position, centroid_position)
logger.info('-'*60+'\n')
if sample_num < num_samples:
sample_num += 1
else:
active = False
if realtime_plot:
print('\n**** Close plot window to continue. ****\n')
from os import system
import WalabotAPI as wlbt
def print_target_short(t):
print('Target #{}\nx = {}\ny = {}\nz = {}\n'.format(
i + 1, t.xPosCm, t.yPosCm, t.zPosCm))
def print_target(t):
pos = '(%f, %f, %f) cm' % (t.xPosCm, t.yPosCm, t.zPosCm)
print('%s %s' % (pos, 0))
wlbt.Init() # load the WalabotSDK to the Python wrapper
wlbt.SetSettingsFolder() # set the path to the essetial database files
wlbt.ConnectAny() # establishes communication with the Walabot
wlbt.SetProfile(wlbt.PROF_SENSOR) # set scan profile out of the possibilities
wlbt.SetDynamicImageFilter(wlbt.FILTER_TYPE_MTI) # specify filter to use
wlbt.Start() # starts Walabot in preparation for scanning
while True:
wlbt.Trigger() # initiates a scan and records signals
targets = wlbt.GetSensorTargets() # provides a list of identified targets
system('cls' if platform == 'win32' else 'clear') # clear the terminal
for i, t in enumerate(targets):
print_target(t)
def stopAndDisconnectWalabot():
""" Stops Walabot and disconnect the device.
"""
wlbt.Stop()
wlbt.Disconnect()
def prep_plot(stdscr, posmap):
'''Animated plot'''
if stdscr is not None:
stdscr.nodelay(1)
# Capture first image
wb.Trigger()
M, _, _, _, _ = wb.GetRawImageSlice()
s = np.array([val for phi in M for val in phi], dtype=np.float32)
fig = plt.figure()
sm = ScalarMappable(cmap='coolwarm')
ax = fig.add_subplot(111, facecolor=sm.to_rgba(0))
sm = ScalarMappable(cmap='coolwarm')
p = ax.scatter(posmap[0], posmap[1], s=posmap[2], c=s, cmap='coolwarm')
def plot_update(image):
'''Update plot colors'''
# Update image colors according to return signal strength
p.set_color(sm.to_rgba(image))
return (p, )
def get_image():
'''Get image from Walabot server'''
threshold = 1.0
active = True
while active:
wb.Trigger()
M, _, _, _, _ = wb.GetRawImageSlice()
image = np.array([val for phi in M for val in phi],
dtype=np.float32)
yield image
if os.name == 'nt':
if kbhit():
key = ord(getch())
else:
key = -1
else:
key = stdscr.getch()
if key != -1:
if key == ord('q'):
active = False
elif key == 224:
if os.name == 'nt':
key = ord(getch())
else:
key = stdscr.getch()
if key == 72:
threshold += 0.1
elif key == 73:
threshold += 1.0
elif key == 80:
threshold -= 0.1
elif key == 81:
threshold -= 1.0
# if is_set == 'set':
# print("Tracker threshold = {0:f}".format(threshold))
ani = animation.FuncAnimation(fig,
func=plot_update,
frames=get_image,
repeat=False,
interval=0,
blit=True)
try:
plt.show()
except:
pass