def __init__(
self,
radar_config,
angle_res=1,
angle_range=90,
origin_at_bottom_center=True,
use_float32=False,
):
super(RadarFrame, self).__init__()
self.cfg = radar_config
#Beamforming params
self.bins_processed = self.cfg['profiles'][0][
'adcSamples'] #radar_cube.shape[0]
self.virt_ant = self.cfg['numLanes'] * len(
self.cfg['chirps']) #radar_cube.shape[1]
self.doppler_bins = self.cfg['numChirps'] // len(
self.cfg['chirps']) #radar_cube.shape[2]
self.angle_res = angle_res
self.angle_range = angle_range
self.angle_bins = (self.angle_range * 2) // self.angle_res + 1
self.num_vec, self.steering_vec = dsp.gen_steering_vec(
self.angle_range, self.angle_res, self.virt_ant)
#Properties
self.__range_azimuth_dirty = True
self.__range_azimuth = np.zeros(
(self.bins_processed, self.angle_bins),
dtype=np.complex64 if use_float32 else np.complex_)
self.__beam_weights = np.zeros(
(self.virt_ant, self.bins_processed),
dtype=np.complex64 if use_float32 else np.complex_)
self.__range_doppler = None
self.__raw_cube = None
self.__range_cube = None
self.__flip_ra = origin_at_bottom_center
ANGLE_RES = 1
ANGLE_RANGE = 90
ANGLE_BINS = (ANGLE_RANGE * 2) // ANGLE_RES + 1
BINS_PROCESSED = 112
# Read in adc data file
load_data = True
if load_data:
adc_data = np.fromfile('./data/circle.bin', dtype=np.uint16)
adc_data = adc_data.reshape(NUM_FRAMES, -1)
all_data = np.apply_along_axis(DCA1000.organize, 1, adc_data, num_chirps=NUM_CHIRPS*2, num_rx=NUM_RX, num_samples=NUM_ADC_SAMPLES)
# Start DSP processing
range_azimuth = np.zeros((ANGLE_BINS, BINS_PROCESSED))
num_vec, steering_vec = dsp.gen_steering_vec(ANGLE_RANGE, ANGLE_RES, VIRT_ANT)
tracker = EKF()
for frame in all_data:
""" 1 (Range Processing) """
# --- range fft
radar_cube = dsp.range_processing(frame)
""" 2 (Capon Beamformer) """
# --- static clutter removal
mean = radar_cube.mean(0)
radar_cube = radar_cube - mean
# --- capon beamforming
def plot_heatmap(adc_data_path, bin_start=4, bin_end=14, diff=False, remove_clutter=True, cumulative=False):
num_bins = bin_end - bin_start
npy_azi = np.zeros((numFrames, ANGLE_BINS, num_bins))
npy_ele = np.zeros((numFrames, ANGLE_BINS, num_bins))
adc_data = np.fromfile(adc_data_path, dtype=np.int16)
adc_data = adc_data.reshape(numFrames, -1)
adc_data = np.apply_along_axis(DCA1000.organize, 1, adc_data, num_chirps=numChirpsPerFrame,
num_rx=numRxAntennas, num_samples=numADCSamples)
# Start DSP processing
range_azimuth = np.zeros((ANGLE_BINS, BINS_PROCESSED))
range_elevation = np.zeros((ANGLE_BINS, BINS_PROCESSED))
num_vec, steering_vec = dsp.gen_steering_vec(ANGLE_RANGE, ANGLE_RES, VIRT_ANT_AZI)
num_vec_ele, steering_vec_ele = dsp.gen_steering_vec(ANGLE_RANGE, ANGLE_RES, VIRT_ANT_ELE)
if cumulative:
cum_azi = np.zeros((ANGLE_BINS, num_bins))
cum_ele = np.zeros((ANGLE_BINS, num_bins))
if diff:
pre_h = np.zeros((ANGLE_BINS, num_bins))
pre_e = np.zeros((ANGLE_BINS, num_bins))
for frame_index in range(numFrames):
""" 1 (Range Processing) """
frame = adc_data[frame_index]
# --- range fft
radar_cube = dsp.range_processing(frame)
# range_bin_idx = 5
# radar_cube to
""" 2 (Capon Beamformer) """
# --- static clutter removal
# --- Do we need ?
if remove_clutter:
mean = radar_cube.mean(0)
radar_cube = radar_cube - mean
# --- capon beamforming
beamWeights = np.zeros((VIRT_ANT_AZI, BINS_PROCESSED), dtype=np.complex_)
radar_cube_azi = np.concatenate((radar_cube[0::numTxAntennas, ...], radar_cube[1::numTxAntennas, ...]), axis=1)
# 4 virtual antenna
# radar_cube_azi = radar_cube[0::numTxAntennas, ...]
# Note that when replacing with generic doppler estimation functions, radarCube is interleaved and
# has doppler at the last dimension.
for i in range(BINS_PROCESSED):
range_azimuth[:, i], beamWeights[:, i] = dsp.aoa_capon(radar_cube_azi[:, :, i].T, steering_vec,
magnitude=True)
# --- capon beamforming elevation (3,5)
beamWeights_ele = np.zeros((VIRT_ANT_ELE, BINS_PROCESSED), dtype=np.complex_)
radar_cube_ele = np.concatenate(
(radar_cube[0::numTxAntennas, 2:3, ...], radar_cube[2::numTxAntennas, 0:1, ...]), axis=1)
for i in range(BINS_PROCESSED):
range_elevation[:, i], beamWeights_ele[:, i] = dsp.aoa_capon(radar_cube_ele[:, :, i].T, steering_vec_ele,
magnitude=True)
""" 3 (Object Detection) """
heatmap_azi = np.log2(range_azimuth[:, bin_start:bin_end])
heatmap_ele = np.log2(range_elevation[:, bin_start:bin_end])
if cumulative:
cum_azi += heatmap_azi
cum_ele += heatmap_ele
if diff:
heatmap_azi = heatmap_azi - pre_h
heatmap_ele = heatmap_ele - pre_e
pre_h = heatmap_azi
pre_e = heatmap_ele
# normalize
heatmap_azi = heatmap_azi / heatmap_azi.max()
heatmap_ele = heatmap_ele / heatmap_ele.max()
npy_azi[frame_index] = heatmap_azi
npy_ele[frame_index] = heatmap_ele
return npy_azi, npy_ele
def plot_heatmap_capon(adc_data_path, save_path, bin_start=4, bin_end=14, diff=False, is_log=False,
remove_clutter=True,
cumulative=False):
num_bins = bin_end - bin_start
npy_azi = np.zeros((numFrames, ANGLE_BINS, num_bins))
npy_ele = np.zeros((numFrames, ANGLE_BINS, num_bins))
adc_data = np.fromfile(adc_data_path, dtype=np.int16)
adc_data = adc_data.reshape(numFrames, -1)
adc_data = np.apply_along_axis(DCA1000.organize, 1, adc_data, num_chirps=numChirpsPerFrame,
num_rx=numRxAntennas, num_samples=numADCSamples)
# Start DSP processing
range_azimuth = np.zeros((ANGLE_BINS, BINS_PROCESSED))
range_elevation = np.zeros((ANGLE_BINS, BINS_PROCESSED))
num_vec, steering_vec = dsp.gen_steering_vec(ANGLE_RANGE, ANGLE_RES, VIRT_ANT_AZI)
num_vec_ele, steering_vec_ele = dsp.gen_steering_vec(ANGLE_RANGE, ANGLE_RES, VIRT_ANT_ELE)
if cumulative:
cum_azi = np.zeros((ANGLE_BINS, num_bins))
cum_ele = np.zeros((ANGLE_BINS, num_bins))
if diff:
pre_h = np.zeros((ANGLE_BINS, num_bins))
pre_e = np.zeros((ANGLE_BINS, num_bins))
for frame_index in range(numFrames):
frame = adc_data[frame_index]
radar_cube = dsp.range_processing(frame)
# virtual antenna arrangement
radar_cube = arange_tx(radar_cube, num_tx=numTxAntennas, vx_axis=1, axis=0)
# --- static clutter removal
if remove_clutter:
mean = radar_cube.mean(0)
radar_cube = radar_cube - mean
# --- capon beamforming
radar_cube_azi = radar_cube[:, VIRT_ANT_AZI_INDEX, :]
radar_cube_ele = radar_cube[:, VIRT_ANT_ELE_INDEX, :]
# Note that when replacing with generic doppler estimation functions, radarCube is interleaved and
# has doppler at the last dimension.
for i in range(BINS_PROCESSED):
range_azimuth[:, i], _ = dsp.aoa_capon(radar_cube_azi[:, :, i].T, steering_vec,
magnitude=True)
range_elevation[:, i], _ = dsp.aoa_capon(radar_cube_ele[:, :, i].T, steering_vec_ele,
magnitude=True)
""" 3 (Object Detection) """
if is_log:
heatmap_azi = 20 * np.log10(range_azimuth[:, bin_start:bin_end])
heatmap_ele = 20 * np.log10(range_elevation[:, bin_start:bin_end])
else:
heatmap_azi = range_azimuth[:, bin_start:bin_end]
heatmap_ele = range_elevation[:, bin_start:bin_end]
if cumulative:
cum_azi += heatmap_azi
cum_ele += heatmap_ele
if diff:
heatmap_azi = heatmap_azi - pre_h
heatmap_ele = heatmap_ele - pre_e
pre_h = heatmap_azi
pre_e = heatmap_ele
# normalize
# heatmap_azi = heatmap_azi / heatmap_azi.max()
# heatmap_ele = heatmap_ele / heatmap_ele.max()
npy_azi[frame_index] = heatmap_azi
npy_ele[frame_index] = heatmap_ele
save_path_azi = save_path + "_azi"
save_path_ele = save_path + "_ele"
np.save(save_path_azi, npy_azi)
np.save(save_path_ele, npy_ele)
print("{} npy file saved!".format(save_path))
# range_doppler_flag = False : Will accumulate using thresholded range axis
# =============================================================================
range_doppler_flag = True
capon_flag = True
if range_doppler_flag:
uDoppler_processed = rs.range_doppler_selection(rangeDoppler,
threshold=0.75)
# mv.stitch_visualizer(uDoppler_processed, chirpPeriod, doppler_resolution) # optional cmap_plot='viridis'
else:
uDoppler_range_pro = rs.range_selection(rangeDoppler, threshold=0.75)
# mv.stitch_visualizer(uDoppler_range_pro, chirpPeriod, doppler_resolution) # optional cmap_plot='viridis'
# --- Get theta info ---
# Generate Steering Vector
num_vec, steering_vector = dsp.gen_steering_vec(90, 1, 8)
# Integrate Theta info into RDC
scan_aoa_capon = np.zeros((numFrames, uDoppler.shape[1], 181))
for f in range(numFrames):
for r in range(uDoppler.shape[1]):
if capon_flag:
scan_aoa_capon[f, r, :], _ = dsp.aoa_capon(thetaData[f,
r, :, :],
steering_vector,
magnitude=True)
else:
scan_aoa_capon[f, r, :] = np.abs(
dsp.aoa_bartlett(steering_vector,
np.sum(thetaData[f, r, :, :],
axis=1,
def featureExtraction(folder, optPath):
"""
Return opt: numFrame, 3 (heatmaps), 46*46(angle bin), nLoopsPerFrame
"""
startTime = ''
with open(folder+'adc_data_Raw_LogFile.csv') as csv_file:
csv_reader = csv.reader(csv_file, delimiter=',')
for row in csv_reader:
if 'Capture start time' in str(row):
startTimeRow = row
break
startTimeStr = startTimeRow[0].split(' - ')[1]
timestamp = datetime.datetime.timestamp(parser.parse(startTimeStr))
timestampList = []
filename = folder + 'adc_data.bin'
numFrames = 640
numADCSamples = 512
numTxAntennas = 2
numRxAntennas = 4
numLoopsPerFrame = 76
numChirpsPerFrame = numTxAntennas * numLoopsPerFrame
BINS_PROCESSED = 55
ANGLE_RES = 1
ANGLE_RANGE = 70
ANGLE_BINS = (ANGLE_RANGE * 2) // ANGLE_RES + 1
reso = dsp.range_resolution(numADCSamples, dig_out_sample_rate=6000, freq_slope_const=39.010)
reso = round(reso[0], 4)
adc_data = np.fromfile(filename, dtype=np.uint16)
# (1) Load Data
if adc_data.shape[0]<398458880:
padSize = 398458880 - adc_data.shape[0]
print("padded size: ", padSize)
adc_data = np.pad(adc_data, padSize)[padSize:]
adc_data = adc_data.reshape(numFrames, -1)
adc_data = np.apply_along_axis(DCA1000.organize, 1, adc_data, num_chirps=numChirpsPerFrame,
num_rx=numRxAntennas, num_samples=numADCSamples)
heatmaps = np.zeros((adc_data.shape[0], ANGLE_BINS, ANGLE_BINS), dtype = 'float32')
numVirAntHori = 3
numVirAntVer = 4
# (2) Start DSP processing
num_vec_4va, steering_vec_4va = dsp.gen_steering_vec(ANGLE_RANGE, ANGLE_RES, numVirAntVer)
num_vec_3va, steering_vec_3va = dsp.gen_steering_vec(ANGLE_RANGE, ANGLE_RES, numVirAntHori)
# (3) Process each frame
for i, frame in enumerate(adc_data):
timestamp+=0.033
timestampList.append(timestamp)
range_azimuth = np.zeros((ANGLE_BINS, BINS_PROCESSED-10))
range_azimuth2 = np.zeros((ANGLE_BINS, BINS_PROCESSED-10))
range_elevation = np.zeros((ANGLE_BINS, BINS_PROCESSED-10))
# Range Processing
radar_cube = dsp.range_processing(frame, window_type_1d=Window.BLACKMAN)
""" (Capon Beamformer) """
# --- static clutter removal / normalize
mean = radar_cube.mean(0)
radar_cube = radar_cube - mean
# --- capon beamforming
beamWeights_azimuth = np.zeros((numVirAntHori, BINS_PROCESSED), dtype=np.complex_)
beamWeights_azimuth2 = np.zeros((numVirAntHori, BINS_PROCESSED), dtype=np.complex_)
beamWeights_elevation = np.zeros((numVirAntVer, BINS_PROCESSED), dtype=np.complex_)
# Separate TX, rx 1234, vrx1234
radar_cube = np.concatenate((radar_cube[0::2, ...], radar_cube[1::2, ...]), axis=1)
# Note that when replacing with generic doppler estimation functions, radarCube is interleaved and
# has doppler at the last dimension.
# Range bin processed
for j in range(10,BINS_PROCESSED):
range_azimuth[:, j-10], beamWeights_azimuth[:, j-10] = dsp.aoa_capon( radar_cube[:, 1:4 , j-10].T, steering_vec_3va, magnitude=True)
range_azimuth2[:, j-10], beamWeights_azimuth2[:, j-10] = dsp.aoa_capon( radar_cube[:, 5:8 , j-10].T, steering_vec_3va, magnitude=True)
range_elevation[:, j-10], beamWeights_elevation[:, j-10] = dsp.aoa_capon( radar_cube[:, [0,4,1,5] , j-10].T, steering_vec_4va, magnitude=True)
# normalize
prescale_factor = 10000000
range_azimuth = scale(range_azimuth/prescale_factor, axis = 1)
range_azimuth2 = scale(range_azimuth2/prescale_factor, axis = 1)
range_elevation = scale(range_elevation/prescale_factor, axis = 1)
range_azimuth = resize(range_azimuth, (ANGLE_BINS, ANGLE_BINS/3))
range_azimuth2 = resize(range_azimuth2, (ANGLE_BINS, ANGLE_BINS/3))
range_elevation = resize(range_elevation, (ANGLE_BINS, ANGLE_BINS/3))
heatmaps[i,:,:] = np.concatenate((range_azimuth, range_azimuth2, range_elevation), axis = 1)
# opt
heatmaps=heatmaps.astype('float32')
if np.isnan(heatmaps).sum() + np.isinf(heatmaps).sum() > 0:
print('dtype: ', heatmaps.dtype, ', has NAN or INF error')
else:
print('dtype: ', heatmaps.dtype)
if optPath != '':
# if not os.path.isdir(optPath + '//'):
# os.mkdir(optPath + '//')
np.savez(optPath+'.npz', heatmaps=heatmaps, timestampList=timestampList)
return heatmaps, np.array(timestampList), reso
# (1) Reading in adc data
if loadData:
adc_data = np.fromfile(adc_data_path, dtype=np.int16)
adc_data = adc_data.reshape(numFrames, -1)
adc_data = np.apply_along_axis(DCA1000.organize,
1,
adc_data,
num_chirps=numChirpsPerFrame,
num_rx=numRxAntennas,
num_samples=numADCSamples)
dataCube = np.copy(adc_data)
print("Data Loaded!")
# Start DSP processing
num_vec, steering_vec = dsp.gen_steering_vec(ANGLE_RANGE, ANGLE_RES,
VIRT_ANT_AZI)
num_vec_ele, steering_vec_ele = dsp.gen_steering_vec(
ANGLE_RANGE, ANGLE_RES, VIRT_ANT_ELE)
fig, axes = plt.subplots(1,
4,
figsize=(ANGLE_BINS // 5,
BINS_PROCESSED // 5 * 4))
# fig, axes = plt.subplots(1, 2, figsize=(ANGLE_BINS // 5, BINS_PROCESSED // 5 * 2))
frame_index = 0
# stored np array
cum_h = np.zeros((ANGLE_BINS, BINS_PROCESSED))
cum_e = np.zeros((ANGLE_BINS, BINS_PROCESSED))
# cum_h = np.zeros((ANGLE_BINS, BINS_PROCESSED), dtype=np.complex_)
