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_)