def __aoa_capon_process(self):
radar_cube = self.range_cube
for jj in range(self.bins_processed):
self.__range_azimuth[
jj, :], self.__beam_weights[:, jj] = dsp.aoa_capon(
radar_cube[jj], self.steering_vec)
self.__range_azimuth_dirty = False
# --- 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
beamWeights = np.zeros((VIRT_ANT, BINS_PROCESSED), dtype=np.complex_)
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.
for i in range(BINS_PROCESSED):
range_azimuth[:,i], beamWeights[:,i] = dsp.aoa_capon(radar_cube[:, :, i].T, steering_vec, magnitude=True)
""" 3 (Object Detection) """
heatmap_log = np.log2(range_azimuth)
# --- cfar in azimuth direction
first_pass, _ = np.apply_along_axis(func1d=dsp.ca_,
axis=0,
arr=heatmap_log,
l_bound=1.5,
guard_len=4,
noise_len=16)
# --- cfar in range direction
second_pass, noise_floor = np.apply_along_axis(func1d=dsp.ca_,
axis=0,
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))
# 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,
keepdims=True),
axis=0)).squeeze()
scan_aoa_capon = 20 * np.log10(scan_aoa_capon)
mv.range_azimuth_visualizer(scan_aoa_capon)
# # --- Begin PCA Process ---
# nFeatures = 3
# # Calulate mean value over all dimensions
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
# --- 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
beamWeights = np.zeros((VIRT_ANT, BINS_PROCESSED))
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.
for i in range(BINS_PROCESSED):
range_azimuth[:, i], beamWeights[:, i] = dsp.aoa_capon(
steering_vec, radar_cube[:, :, i].T)
range_azimuth = np.abs(range_azimuth)
""" 3 (Object Detection) """
heatmap_log = np.log2(range_azimuth)
# --- cfar in azimuth direction
first_pass, _ = np.apply_along_axis(func1d=dsp.ca_,
axis=0,
arr=heatmap_log,
l_bound=1.5,
guard_len=4,
noise_len=16)
# --- cfar in range direction
second_pass, noise_floor = np.apply_along_axis(func1d=dsp.ca_,
axis=0,
radar_cube_azi = [radar_cube[:, VIRT_ANT_AZI_INDEX, :]]
radar_cube_ele = [radar_cube[:, VIRT_ANT_ELE_INDEX, :]]
stack_azi = []
stack_ele = []
if mode == "capon":
for cube_azi in radar_cube_azi:
range_azimuth = np.zeros((ANGLE_BINS, BINS_PROCESSED))
# range_azimuth = np.zeros((ANGLE_BINS, BINS_PROCESSED), dtype=np.complex_)
beamWeights = np.zeros((VIRT_ANT_AZI, BINS_PROCESSED),
dtype=np.complex_)
for i in range(BIN_RANG_S, BIN_RANG_E):
r_i = i - BIN_RANG_S
range_azimuth[:, r_i], beamWeights[:, r_i] = dsp.aoa_capon(
cube_azi[:, :, i].T, steering_vec, magnitude=True)
stack_azi.append(range_azimuth)
for cube_ele in radar_cube_ele:
range_elevation = np.zeros((ANGLE_BINS, BINS_PROCESSED))
# range_elevation = np.zeros((ANGLE_BINS, BINS_PROCESSED), dtype=np.complex_)
beamWeights_ele = np.zeros((VIRT_ANT_ELE, BINS_PROCESSED),
dtype=np.complex_)
for i in range(BIN_RANG_S, BIN_RANG_E):
r_i = i - BIN_RANG_S
range_elevation[:,
r_i], beamWeights_ele[:,
r_i] = dsp.aoa_capon(
cube_ele[:, :,
i].T,
steering_vec_ele,