diff --git a/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/.gitignore b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/.gitignore new file mode 100644 index 0000000..f3f6814 --- /dev/null +++ b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/.gitignore @@ -0,0 +1,2 @@ +__pycache__ +.idea/ \ No newline at end of file diff --git a/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/ConfigureRadar.py b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/ConfigureRadar.py new file mode 100644 index 0000000..7c96537 --- /dev/null +++ b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/ConfigureRadar.py @@ -0,0 +1,105 @@ +import numpy as np +# from numba.experimental import jitclass +from mat4py import loadmat +import pdb + +class radar(): + + """ + class to define the radar object with it's settings and to extract time intervals for updating the scene. + Also contains voxel filter specs + """ + def __init__(self, radartype): + self.radartype = radartype + if radartype == "ti_cascade": + self.center = np.array([0.0, 4.0]) # center of radar + self.elv = np.array([0.5]) # self position in z-axis + self.orientation = 90 # orientation of radar + + self.f = 77e9 + self.B = 0.256e9 # Bandwidth + self.c = 3e8 + self.N_sample = 256 + self.samp_rate = 15e6 + self.doppler_mode = 1 + self.chirps = 3 # 128 + self.nRx = 86 #16 # number of antennas(virtual antennas included, AOA dim) + self.noise_amp = 0.005 #0.0001(concrete+metal) # 0.00001(metal) #0.005(after skyward data) + self.gain = 10 ** (105 / 10) #190(concrete+metal) # 210(metal) + self.angle_fft_size = 256 + + self.range_res = self.c / (2 * self.B) # range resolution + self.max_range = self.range_res * self.N_sample + + self.idle = 0 ## Idle time + self.chirpT = self.N_sample / self.samp_rate ## Time of chirp + self.chirp_rep = 12*27e-6 + + Ts = 1 / self.samp_rate + self.t = np.arange(0, self.chirpT, Ts) + self.tau_resolution = 1 / self.B + self.k = self.B / self.chirpT + + self.voxel_theta = 2.0 # 0.5 # 0.1 + self.voxel_phi = 2.0 # 0.5 # 0.1 + self.voxel_rho = 0.05 # 0.1 # 0.05 + + elif radartype == "radarbook": + self.center = np.array([0.0, 4.0]) # center of radar + self.elv = np.array([0.5]) # self position in z-axis + self.orientation = 90 # orientation of radar + + self.f = 24e9 + self.B = 0.250e9 # Bandwidth + self.c = 3e8 + self.N_sample = 256 + self.samp_rate = 1e6 + self.doppler_mode = 1 + self.chirps = 128 + self.nRx = 8 # number of antennas(virtual antennas included, AOA dim) + self.noise_amp = 0.005 #0.0001(concrete+metal) # 0.00001(metal) #0.005(after skyward data) + self.gain = 10 ** (105 / 10) #190(concrete+metal) # 210(metal) + self.angle_fft_size = 256 + + self.range_res = self.c / (2 * self.B) # range resolution + self.max_range = self.range_res * self.N_sample + + + self.chirpT = self.N_sample / self.samp_rate ## Time of chirp + self.chirp_rep = 0.75e-3 + self.idle = self.chirp_rep - self.chirpT## Idle time + + Ts = 1 / self.samp_rate + self.t = np.arange(0, self.chirpT, Ts) + self.tau_resolution = 1 / self.B + self.k = self.B / self.chirpT + + self.voxel_theta = 2 # 0.5 # 0.1 + self.voxel_phi = 2 # 0.5 # 0.1 + self.voxel_rho = 0.05 # 0.1 # 0.05 + + else: + raise Exception("Incorrect radartype selected") + + def get_noise(self): + if self.radartype == "ti_cascade": + # noise_prop = loadmat('/radar-imaging-dataset/mmfn_project/mmfn_scripts/team_code/e2e_agent_sem_lidar2shenron_package/noise_data/noise_adc.mat') + # real_fft_ns = np.random.normal(noise_prop['noise_mean_real'], noise_prop['noise_std_real']).T + # complex_fft_ns = np.random.normal(noise_prop['noise_mean_real'], noise_prop['noise_std_real']).T + # final_noise = real_fft_ns + 1j * complex_fft_ns + # # signal_Noisy = np.fft.ifft(final_noise, radar.N_sample, 1) #* 10**4.5 + # # signal_Noisy = final_noise + # signal_Noisy = 0*final_noise + + # for low resolution 16 channels + signal_Noisy = np.random.normal(0,1,size=(self.nRx,self.N_sample)) + signal_Noisy = 0*(signal_Noisy + 1j*signal_Noisy) + + elif self.radartype == "radarbook": + signal_Noisy = np.random.normal(0,1,size=(self.nRx,self.N_sample)) + signal_Noisy = 0*(signal_Noisy + 1j*signal_Noisy) + + else: + raise Exception("Incorrect radartype selected") + + return signal_Noisy \ No newline at end of file diff --git a/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/README.md b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/README.md new file mode 100644 index 0000000..f05c825 --- /dev/null +++ b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/README.md @@ -0,0 +1,16 @@ +# SHENRON: Radar Simulation +Packaging shenron into minimal working code + +To run the simulation, follow these steps: + +1. Open the `simulator_config.yaml` file. +2. Add the file paths for the input and output files in the appropriate fields. For example: + + ```yaml + PATHS : + LIDAR_PATH : "/home/Kshitiz/" + LIDAR_FOLDERS : ["semantic_lidar"] + OUT_PATH : "/home/Kshitiz/semantic_lidar/" +3. Run the simulation using main file + ```python + python main.py diff --git a/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/__init__.py b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/carla_shenron_config.yaml b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/carla_shenron_config.yaml new file mode 100644 index 0000000..7f34122 --- /dev/null +++ b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/carla_shenron_config.yaml @@ -0,0 +1,2 @@ +INPUT_PATH : "/radar-imaging-dataset/Jason/mmfn/data1/nss_0702/Town04_short/" +OUTPUT_PATH : "/radar-imaging-dataset/Jason/mmfn/data1/nss_0702/Town04_short/" \ No newline at end of file diff --git a/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/lidar.py b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/lidar.py new file mode 100644 index 0000000..d4ab159 --- /dev/null +++ b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/lidar.py @@ -0,0 +1,229 @@ +import sys +import os +# sys.path.append("../") +sys.path.append('radar-imaging-dataset/carla_garage_radar/team_code/e2e_agent_sem_lidar2shenron_package/') + +import numpy as np +from e2e_agent_sem_lidar2shenron_package.path_config import * +from e2e_agent_sem_lidar2shenron_package.ConfigureRadar import radar +from e2e_agent_sem_lidar2shenron_package.shenron.Sceneset import * +from e2e_agent_sem_lidar2shenron_package.shenron.heatmap_gen_fast import * +# from pointcloud_raytracing.pointraytrace import ray_trace +import scipy.io as sio +from e2e_agent_sem_lidar2shenron_package.lidar_utils import * +import time +import shutil +import pdb + +def map_carla_semantic_lidar_latest(carla_sem_lidar_data): + ''' + Function to map material column in the collected carla ray_cast_shenron to shenron input + ''' + # CARLA 0.9.16 [x, y, z, intensity, cos, obj, tag] + # We want [x, y, z, tag] + carla_sem_lidar_data_crop = carla_sem_lidar_data[:, (0, 1, 2, 6)] + temp_list = np.array([0, 4, 2, 0, 11, 5, 0, 0, 1, 8, 12, 3, 7, 10, 0, 1, 0, 12, 6, 0, 0, 0, 0]) + + col = temp_list[(carla_sem_lidar_data_crop[:, 3].astype(int))] + carla_sem_lidar_data_crop[:, 3] = col + + return carla_sem_lidar_data_crop + +# def map_carla_semantic_lidar(carla_sem_lidar_data): +# ''' +# Function to map material column in the collected carla ray_cast_shenron to shenron input +# ''' +# # print(carla_sem_lidar_data.shape()) +# carla_sem_lidar_data_crop = carla_sem_lidar_data[:, (0, 1, 2, 5)] +# carla_sem_lidar_data_crop[:, 3] = carla_sem_lidar_data_crop[:, 3]-1 +# carla_sem_lidar_data_crop[carla_sem_lidar_data_crop[:, 3]>18, 3] = 255. +# carla_sem_lidar_data_crop[carla_sem_lidar_data_crop[:, 3]<0, 3] = 255. +# carla_sem_lidar_data_crop[:, (0, 1, 2)] = carla_sem_lidar_data_crop[:, (0, 2, 1)] +# return carla_sem_lidar_data_crop + +def check_save_path(save_path): + if not os.path.exists(save_path): + os.makedirs(save_path) + return + +def rotate_points(points, angle): + rotMatrix = np.array([[np.cos(np.deg2rad(angle)), np.sin(np.deg2rad(angle)), 0] + , [- np.sin(np.deg2rad(angle)), np.cos(np.deg2rad(angle)), 0] + , [0, 0, 1]]) + return np.matmul(points, rotMatrix) + +def Cropped_forRadar(pc, veh_coord, veh_angle, radarobj): + """ + Removes Occlusions and calculates loss for each point + """ + + skew_pc = rotate_points(pc[:, 0:3] , veh_angle ) + # skew_pc = np.vstack(((skew_pc ).T, pc[:, 3], pc[:, 5])).T #x,y,z,speed,material + skew_pc = np.vstack(((skew_pc ).T, pc[:, 3], pc[:, 5],pc[:,6])).T #x,y,z,speed,material, cosines + + rowy = np.where((skew_pc[:, 1] > 0.8)) + new_pc = skew_pc[rowy, :].squeeze(0) + + new_pc = new_pc[new_pc[:,4]!=0] + + new_pc = new_pc[(new_pc[:,0]<50)*(new_pc[:,0]>-50)] + new_pc = new_pc[(new_pc[:,1]<100)] + new_pc = new_pc[(new_pc[:,2]<2)] + + simobj = Sceneset(new_pc) + + [rho, theta, loss, speed, angles] = simobj.specularpoints(radarobj) + print(f"Number of points = {rho.shape[0]}") + return rho, theta, loss, speed, angles + +def run_lidar(sim_config, sem_lidar_frame): + + #restructed lidar.py code + + # lidar_path = f'{base_folder}/{sim_config["CARLA_SHENRON_SEM_LIDAR"]}' + # # lidar_velocity_path = f'{base_folder}/{sim_config["LIDAR_PATH_POINT_VELOCITY"]}/' + # out_path = f'{base_folder}/{sim_config["RADAR_PATH_SIMULATED"]}' + + # if not os.path.exists(out_path): + # os.makedirs(out_path) + + # shutil.copyfile('ConfigureRadar.py',f'{base_folder}/radar_params.py') + + # lidar_files = os.listdir(lidar_path) + # lidar_velocity_files = os.listdir(lidar_velocity_path) + # lidar_files.sort() + # lidar_velocity_files.sort() + + # print(lidar_files) + + #Lidar specific settings + radarobj = radar(sim_config["RADAR_TYPE"]) + # radarobj.chirps = 128 + radarobj.center = np.array([0.0, 0.0]) # center of radar + radarobj.elv = np.array([0.0]) + + # setting the sem lidar inversion angle here + veh_angle = sim_config['INVERT_ANGLE'] + + # all_speeds = [] + + # temp_angles = [] + # temp_rho = [] + # for file_no, file in enumerate(lidar_files): + + # start = time.time() + # if file.endswith('.npy'): # .pcd + # print(file) + + # lidar_file_path = os.path.join(f"{lidar_path}/", file) + # load_pc = np.load(lidar_file_path) + + # load_velocity = np.load(f'{lidar_velocity_path}/{file}') + + # test = map_material(test) + # cos_inc_angle is at index 4 in [x, y, z, intensity, cos, obj, tag] + cosines = sem_lidar_frame[:, 4] + load_pc = sem_lidar_frame + load_pc = map_carla_semantic_lidar_latest(load_pc) + test = new_map_material(load_pc) + + points = np.zeros((np.shape(test)[0], 7)) + # points[:, [0, 1, 2]] = test[:, [0, 2, 1]] + points[:, [0, 1, 2]] = test[:, [1, 0, 2]] + + """ + points mapping + +ve ind 0 == right + +ve ind 1 == +ve depth + +ve ind 2 == +ve height + """ + # add the velocity channel here to the lidar points on the channel number 3 most probably + # points[:, 3] = load_velocity + + points[:, 5] = test[:, 3] + points[:, 6] = cosines + ### if jason carla lidar + # points = np.zeros((np.shape(test)[0], 6)) + # points[:, [0, 1, 2]] = load_pc[:, [0, 1, 2]] + # points[:, 5] = 4 + ########## + + # if USE_DIGITAL_TWIN: + # gt_label = gt[file_no,:] + # points, veh_speeds = create_digital_twin(points, gt_label) ## This also claculates and outputs speed + + # all_speeds.append(veh_speeds) + + # if sim_config["RADAR_MOVING"]: + # # when the radar is moving, we add a negative doppler from all the points + # if INDOOR: + # curr_radar_speed = radar_speeds[file_no,:] + + # cos_theta = (points[:,1]/np.linalg.norm(points[:,:2],axis=1)) + # radial_speed = -np.linalg.norm(curr_radar_speed)*cos_theta + + # points[:,3] += radial_speed + # points[:,5] = 4 ## harcoded + + + Crop_rho, Crop_theta, Crop_loss, Crop_speed, Crop_angles = Cropped_forRadar(points, np.array([0, 0, 0]), veh_angle, radarobj) + + """ DEBUG CODE + spec_angle_thresh = 2*np.pi/180#*(1/rho) + + print(f"Number of points < 2deg = {np.sum(abs(Crop_angles) 0, :] # filter to obtain calibration points + # test = test[test[:, 1] < 10, :] + # test[test[:, 3] != 255, 3] = 5 + + + # amplify = 1 + # after metal tuning - 0.01,0.1,0.001 + conc = 0.03 # .04 # 0.3 # 2 # best # calculated # lucky + steel = 0.1 # 0.99 # 200 + tree = 0.001 # 0.0001 # 0.01 # 0.02 + human = 0.1 + test[test[:, 3] == 255, 3] = 0 # for unlabeled assign 0 + test[test[:, 3] == 0, 3] = 0 # for road->concrete, 0 to suppress the ground rings that show up as objects + test[test[:, 3] == 1, 3] = conc # for sidewalk->concrete + test[test[:, 3] == 2, 3] = conc # for building->concrete + test[test[:, 3] == 3, 3] = conc # for wall->concrete + test[test[:, 3] == 4, 3] = steel # for fence->metal + test[test[:, 3] == 5, 3] = steel # for pole->metal + test[test[:, 3] == 6, 3] = steel # for traffic light->metal + test[test[:, 3] == 7, 3] = steel # for traffic sign->metal + test[test[:, 3] == 8, 3] = tree # for vegetation->trees # just for wall data + test[test[:, 3] == 9, 3] = tree # for terrain->trees # just for wall data + test[test[:, 3] == 10, 3] = 0 # for sky->0 + test[test[:, 3] == 11, 3] = human # for person->metal (reflects significant energy, approx) + test[test[:, 3] == 12, 3] = steel # for rider->metal + test[test[:, 3] == 13, 3] = steel # for car->metal + test[test[:, 3] == 14, 3] = steel # for truck->metal + test[test[:, 3] == 15, 3] = steel # for bus->metal + test[test[:, 3] == 16, 3] = steel # for train->metal + test[test[:, 3] == 17, 3] = steel # for motorbike->metal + test[test[:, 3] == 18, 3] = steel # for bicycle->metal + + return test + +def new_map_material(test): + # map material to index + #0 -> unlabeled + #1 -> wood + #2 -> concrete + #3 -> human + #4 -> metal + + unlabel = 0 + wood = 1 + conc = 2 + human = 3 + metal = 4 + + unlabel_roughness = 0 + wood_roughness = 0.0017 #1.7mm + conc_roughness = 0.0017 #1.7mm + human_roughness = 0.0001 # 100um + metal_roughness = 0.0001 # 100um + + roughness = np.array([unlabel_roughness,wood_roughness,conc_roughness,human_roughness,metal_roughness]) + + unlabel_perm = 1 + wood_perm = 2 + conc_perm = 5.24 + human_perm = 15 + metal_perm = 100000 + + permittivity = np.array([unlabel_perm,wood_perm,conc_perm,human_perm,metal_perm]) + + + test[test[:, 3] == 255, 3] = unlabel # for unlabeled assign 0 + test[test[:, 3] == 0, 3] = unlabel # for road->unlabeled, to suppress the ground rings that show up as objects + test[test[:, 3] == 1, 3] = conc # for sidewalk->concrete + test[test[:, 3] == 2, 3] = conc # for building->concrete + test[test[:, 3] == 3, 3] = conc # for wall->concrete + test[test[:, 3] == 4, 3] = metal # for fence->metal + test[test[:, 3] == 5, 3] = metal # for pole->metal + test[test[:, 3] == 6, 3] = metal # for traffic light->metal + test[test[:, 3] == 7, 3] = metal # for traffic sign->metal + test[test[:, 3] == 8, 3] = wood # for vegetation->trees # just for wall data + test[test[:, 3] == 9, 3] = wood # for terrain->trees # just for wall data + test[test[:, 3] == 10, 3] = unlabel # for sky->0 + test[test[:, 3] == 11, 3] = human # for person->metal (reflects significant energy, approx) + test[test[:, 3] == 12, 3] = metal # for rider->metal + test[test[:, 3] == 13, 3] = metal # for car->metal + test[test[:, 3] == 14, 3] = metal # for truck->metal + test[test[:, 3] == 15, 3] = metal # for bus->metal + test[test[:, 3] == 16, 3] = metal # for train->metal + test[test[:, 3] == 17, 3] = metal # for motorbike->metal + test[test[:, 3] == 18, 3] = metal # for bicycle->metal + + return test + +if __name__ == '__main__': + + gt = sio.loadmat("../gt_11_08_car_round2.mat") + gt = gt["a"] + + + # print(gt_label) + exp_date = "11_08" + lidar_path = f"Y:/media/Kshitiz/RADARIMAGING/radar_simulator_data/{exp_date}_data/{exp_date}_lidar_appended" + lidar_folder = f"test_11_08_car_round2" + # file = "0001.npy" + files = np.array(os.listdir(f"{lidar_path}/{lidar_folder}/")) + + select_array = np.arange(50,51) + for file_idx, file in enumerate(files[select_array]): + lidar_file_path = os.path.join(f"{lidar_path}/{lidar_folder}/", file) + + test = np.load(lidar_file_path) + test = map_material(test) + + points = np.zeros((np.shape(test)[0], 6)) + + points[:, [0, 1, 2]] = test[:, [0, 2, 1]] + + points[:, 5] = test[:, 3] + + gt_label = gt[select_array[file_idx],:] + points = create_digital_twin(points, gt_label) + + pcd = o3d.geometry.PointCloud() + pcd.points = o3d.utility.Vector3dVector(points[:, 0:3]) + o3d.visualization.draw_geometries([pcd]) + + \ No newline at end of file diff --git a/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/main.py b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/main.py new file mode 100644 index 0000000..69d3a75 --- /dev/null +++ b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/main.py @@ -0,0 +1,63 @@ +# -*- coding: utf-8 -*- +""" +Created on Wed Apr 7 16:00:26 2021 + +@author: ksban +""" + +from lidar import run_lidar +import os +import yaml + +def run_shenron(sim_config, base_folder): + print("in run shenron") + # with open('simulator_configs.yaml', 'r') as f: + # sim_config = yaml.safe_load(f) + + # printing the base path here for the check + # print(f"the base folder currently is base/") + + folders = os.listdir(f"{sim_config['BASE_PATH']}/{base_folder}") + folders.sort() + # print(folders) + # run_folders = ["set4_vertical_crossing_2023-10-08-13-07-48", "set3_split_vertical_crossing_2023-10-08-12-56-58", "set2_vertical_crossing_2023-10-08-12-24-52", "set1_vertical_crossing_2023-10-08-12-02-41"] + # return + # run_folders= ["Town01_20_08_10_09_47_16"] + # run_folders= ["Town01_10_08_10_06_43_59"] #with 3 chirps only + for folder in folders: + # if folder in run_folders: + if os.path.isdir(f'{sim_config["BASE_PATH"]}/{base_folder}/{folder}/'): + exec_path = f'{sim_config["BASE_PATH"]}/{base_folder}/{folder}/' + print(f"currently running the folder {exec_path.split('/')[-2]}") + if sim_config["INPUT"] == "lidar": + run_lidar(sim_config, exec_path) + else: + print("Incorrect input in config") + + print("this folder done") + + else: + print("skipping this, not a folder") + continue + + # else: + # continue + +if __name__ == '__main__': + + with open('simulator_configs.yaml', 'r') as f: + sim_config = yaml.safe_load(f) + + # if sim_config["INPUT"] == "lidar": + # run_lidar(sim_config) + # else: + # print("Incorrect input in config") + print("in main") + + base_base_folders = os.listdir(sim_config["BASE_PATH"]) + base_base_folders.sort() + run_folders = ["Town05_tiny", "Town06_short", "Town06_tiny", "Town07_short", "Town07_tiny", "Town10_short", "Town10_tiny",] + for base_folder in base_base_folders: + if base_folder in run_folders: + print(f"currently running the {base_folder}") + run_shenron(sim_config, base_folder) \ No newline at end of file diff --git a/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/map_carla_semantic_lidar_shenron.py b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/map_carla_semantic_lidar_shenron.py new file mode 100644 index 0000000..4ec719d --- /dev/null +++ b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/map_carla_semantic_lidar_shenron.py @@ -0,0 +1,63 @@ +import numpy as np +import os +import yaml +# import run_shenron +# from main import run_shenron + +def check_save_path(save_path): + if not os.path.exists(save_path): + os.makedirs(save_path) + return + +def main(): + + with open('carla_shenron_config.yaml', 'r') as f: + config = yaml.safe_load(f) + + in_path = config["INPUT_PATH"] + out_path = config["OUTPUT_PATH"] + + if not os.path.exists(out_path): + os.makedirs(out_path, exist_ok=True) + + folders = os.listdir(in_path) + folders.sort() + print(folders) + print("folders loaded, mapping now") + + for folder in folders: + if os.path.isdir(f'{in_path}/{folder}'): + print(f"starting {folder}") + out_path_temp = f'{out_path}/{folder}/carla_shenron_sem_lidar' + check_save_path(out_path_temp) + + files = os.listdir(f'{in_path}/{folder}/semantic_lidar/') + files.sort() + + for file in files: + carla_sem_lidar_data = np.load(f'{in_path}/{folder}/semantic_lidar/{file}') + carla_sem_lidar_data = carla_sem_lidar_data[:, (0, 1, 2, 5)] + carla_sem_lidar_data[:, 3] = carla_sem_lidar_data[:, 3]-1 + + carla_sem_lidar_data[carla_sem_lidar_data[:, 3]>18, 3] = 255. + carla_sem_lidar_data[carla_sem_lidar_data[:, 3]<0, 3] = 255. + # print(carla_sem_lidar_data) + carla_sem_lidar_data[:, (0, 1, 2)] = carla_sem_lidar_data[:, (0, 2, 1)] + # print(carla_sem_lidar_data) + # break + # print(np.unique(carla_sem_lidar_data[:, 3])) + # break + np.save(f'{out_path_temp}/{file[:-4]}.npy', carla_sem_lidar_data) + print(f"{folder} done") + else: + print("skipping this, not a directory") + continue + + #call shenron here after saving carla_shenron_semantic_lidar for all the folders + # print("runnning shenron now") + # run_shenron() + + +if __name__ == "__main__": + print("starting map_carla_semantic_lidar_shenron") + main() \ No newline at end of file diff --git a/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/noise_data/noise.mat b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/noise_data/noise.mat new file mode 100644 index 0000000..4605863 Binary files /dev/null and b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/noise_data/noise.mat differ diff --git a/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/noise_data/noise_adc.mat b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/noise_data/noise_adc.mat new file mode 100644 index 0000000..09c28e9 Binary files /dev/null and b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/noise_data/noise_adc.mat differ diff --git a/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/noise_data/noise_with_ground.mat b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/noise_data/noise_with_ground.mat new file mode 100644 index 0000000..6560ca1 Binary files /dev/null and b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/noise_data/noise_with_ground.mat differ diff --git a/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/path_config.py b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/path_config.py new file mode 100644 index 0000000..5e3dc46 --- /dev/null +++ b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/path_config.py @@ -0,0 +1,43 @@ +# class path_config: +# path_csv = "/media/ehdd_8t1/RadarImaging/collab_radar_data/simulator_inputs/sumo_traffic_data/" +# path_ply = "/media/ehdd_8t1/RadarImaging/collab_radar_data/simulator_inputs/point_cloud_maps/" +# path_cad = "/media/ehdd_8t1/RadarImaging/collab_radar_data/simulator_inputs/car_cad_models/" + +path_config = { + "path_csv": "/media/ehdd_8t1/RadarImaging/collab_radar_data/simulator_inputs/sumo_traffic_data/", + "path_ply": "/media/ehdd_8t1/RadarImaging/collab_radar_data/simulator_inputs/point_cloud_maps/", + "path_cad": "/media/ehdd_8t1/RadarImaging/collab_radar_data/simulator_inputs/car_cad_models/", + "path_ped": "/media/ehdd_8t1/RadarImaging/collab_radar_data/simulator_inputs/human_cad_models/" + } + +out_paths = { + "path_tensors": "/media/ehdd_8t1/RadarImaging/collab_radar_data/radar_tensors/", + "path_gt": "/media/ehdd_8t1/RadarImaging/collab_radar_data/ground_truth/", + "path_images": "/media/ehdd_8t1/RadarImaging/collab_radar_data/radar_bev_images/" + } + +dataset_config = { + "dataset_name": "downtown_SD_10thru_50count", #"ucsd_atkinson_1" + "ply_name": "downtown_SD_10_7.ply", #"ucsd_atkinson_10_7.ply" + "csv_name": "downtown_SD_10thru_50count_10Hz_with_cad_id" +} + +ped_dataset_config = { + "dataset_name": "test_11_08_person3", + "npy_name": "test_11_08_person3_segmented", +} + +out_config = { + "save_dataset_name" : "downtown_SD_10thru_50count_1deg_10Hz" +} + +debug_config = { + "save_cropped_path" : "/media/ehdd_8t1/RadarImaging/collab_radar_data/debug/" +} + +# class data_paths: +# path_tensors = "/media/ehdd_8t1/RadarImaging/collab_radar_data/radar_tensors/" +# path_gt = "/media/ehdd_8t1/RadarImaging/collab_radar_data/ground_truth/" +# path_images = "/media/ehdd_8t1/RadarImaging/collab_radar_data/images_no_white/" + + diff --git a/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/shenron/Sceneset.py b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/shenron/Sceneset.py new file mode 100644 index 0000000..f8d80e2 --- /dev/null +++ b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/shenron/Sceneset.py @@ -0,0 +1,546 @@ +from re import A +import open3d as o3d +import os +import numpy as np +import math +import matplotlib.pyplot as plt +import pdb +from e2e_agent_sem_lidar2shenron_package.ConfigureRadar import radar +import sys + +def cart2sph(x, y, z): + hxy = np.hypot(x, y) + r = np.hypot(hxy, z) + el = np.arctan2(z, hxy) + az = np.arctan2(y, x) + return az, el, r + +def sph2cart( az, el, r): + rcos_theta = r * np.cos(el) + x = rcos_theta * np.cos(az) + y = rcos_theta * np.sin(az) + z = r * np.sin(el) + return x, y, z + +class Sceneset(): + + """ + Class with all the functions to modify OSM scenario and thus generate the points for radar heatmap + """ + def __init__(self, pc): + self.points_3D = pc + self.rad_scene = self.points_3D + pass + + def removeocclusion(self, radar): + + row = np.where(self.points_3D.any(axis=1))[0] + self.points_3D = self.points_3D[row, :] + + # to reduce volume to 100x150x25, else voxel filter too big + rowx = np.where((self.points_3D[:, 0] < 50) & (self.points_3D[:, 0] > -50)) # x limits + self.points_3D = self.points_3D[rowx, :].squeeze(0) + rowy = np.where((self.points_3D[:, 1] < 150) & (self.points_3D[:, 1] > 0)) # y limits + self.points_3D = self.points_3D[rowy,:].squeeze(0) + rowz = np.where((self.points_3D[:, 2] < 5) & (self.points_3D[:, 2] > -2)) #z limits + self.points_3D = self.points_3D[rowz,:].squeeze(0) + rowx = [] + rowy = [] + rowz = [] + + self.rad_scene = self.points_3D + sph_v = np.zeros((self.points_3D.shape[0], 3)) + sph_v[:, 0], sph_v[:, 1], sph_v[:, 2] = cart2sph(self.points_3D[:, 0]-radar.center[0], self.points_3D[:, 1]-radar.center[1], self.points_3D[:, 2]-radar.elv) + sphlim = np.array([np.min(sph_v, axis=0), np.max(sph_v, axis=0)]) # limits in all three dimensions in the spherical coordinates + + + pcd2 = o3d.geometry.PointCloud() + pcd2.points = o3d.utility.Vector3dVector(self.rad_scene[:, 0:3]) + # o3d.visualization.draw_geometries([pcd2]) + + + # convert the point cloud into a grid of 0s and 1s + # define voxel size of the grid + phi_res = radar.voxel_phi / 180 * math.pi + theta_res = radar.voxel_theta / 180 * math.pi + rho_res = radar.voxel_rho + + sph_m_phi = np.arange(sphlim[0, 0], sphlim[1, 0] + phi_res,phi_res) + phi_res + sph_m_theta = np.arange( sphlim[0, 1], sphlim[1, 1] + theta_res, theta_res) + theta_res + sph_m_rho = np.arange(sphlim[0, 2], sphlim[1, 2] + rho_res, rho_res) + rho_res + sph_m_size = [len(sph_m_phi), len(sph_m_theta), len(sph_m_rho)] + sph_m = np.zeros(sph_m_size) + vel_m = np.zeros(sph_m_size) + mtl_m = np.zeros(sph_m_size) + + phi_m_idx = np.round_((sph_v[:, 0] - sphlim[0, 0]) / phi_res) # +1 + theta_m_idx = np.round_((sph_v[:, 1] - sphlim[0, 1]) / theta_res) # +1 + rho_m_idx = np.round_((sph_v[:, 2] - sphlim[0, 2]) / rho_res) # +1 + for k_pt in range(0, len(self.points_3D)): + sph_m[int(phi_m_idx[k_pt]), int(theta_m_idx[k_pt]), int(rho_m_idx[k_pt])] = 1 + vel_m[int(phi_m_idx[k_pt]), int(theta_m_idx[k_pt]), int(rho_m_idx[k_pt])] = self.points_3D[k_pt, 3] + mtl_m[int(phi_m_idx[k_pt]), int(theta_m_idx[k_pt]), int(rho_m_idx[k_pt])] = self.points_3D[k_pt, 4] + + visible_sph_m = np.zeros(sph_m.shape) + visible_vel_m = np.zeros(vel_m.shape) + visible_mtl_m = np.zeros(mtl_m.shape) + for kphi in range(0, sph_m_size[0]): + for ktheta in range(0, sph_m_size[1]): + if sph_m[kphi, ktheta, :].any() > 0: + krho = np.where(sph_m[kphi, ktheta, :] > 0)[0][0] + visible_sph_m[kphi, ktheta, krho] = sph_m[kphi, ktheta, krho] + visible_vel_m[kphi, ktheta, krho] = vel_m[kphi, ktheta, krho] + visible_mtl_m[kphi, ktheta, krho] = mtl_m[kphi, ktheta, krho] + + visible_sph_m_idx = np.where(visible_sph_m) + # sph_v_idx = [] + sph_v_idx = np.array([visible_sph_m_idx[0], visible_sph_m_idx[1], visible_sph_m_idx[2]]).T # np.unravel_index( visible_sph_m_idx, sph_m_size) + visible_vel_v = visible_vel_m[visible_sph_m_idx] + visible_mtl_v = visible_mtl_m[visible_sph_m_idx] + visible_sph_v = np.array([sph_m_phi[sph_v_idx[:, 0]], sph_m_theta[sph_v_idx[:, 1]], sph_m_rho[sph_v_idx[:, 2]]]).T + + visible_cart_v = np.zeros(visible_sph_v.shape) + [visible_cart_v[:, 0], visible_cart_v[:, 1], visible_cart_v[:, 2]] = sph2cart(visible_sph_v[:, 0], visible_sph_v[:, 1], visible_sph_v[:, 2]) + self.rad_scene = np.vstack((visible_cart_v[:, :].T, visible_vel_v, visible_mtl_v)).T+np.hstack((radar.center, radar.elv, np.array([0, 0]))) + + + # pcd2 = o3d.geometry.PointCloud() + # pcd2.points = o3d.utility.Vector3dVector(self.rad_scene[:, 0:3]) + + # o3d.visualization.draw_geometries([pcd2]) + + + # ground_refl = np.vstack((visible_cart_v[:, :].T, visible_vel_v, visible_mtl_v)).T+np.array(radar.center + [1] + [0, 0]) + # self.rad_scene = np.append(self.rad_scene, ground_refl*np.array([1, 1, -1, 1, 1]), axis=0) + + def removeocclusion_hiddenpoint(self, radar): + row = np.where(self.points_3D.any(axis=1))[0] + self.points_3D = self.points_3D[row, :] + + # to reduce volume to 100x150x25, else voxel filter too big + rowx = np.where((self.points_3D[:, 0] < 50) & (self.points_3D[:, 0] > -50)) # x limits + self.points_3D = self.points_3D[rowx, :].squeeze(0) + rowy = np.where((self.points_3D[:, 1] < 150) & (self.points_3D[:, 1] > 0)) # y limits + self.points_3D = self.points_3D[rowy,:].squeeze(0) + rowz = np.where((self.points_3D[:, 2] < 5) & (self.points_3D[:, 2] > -2)) #z limits + self.points_3D = self.points_3D[rowz,:].squeeze(0) + rowx = [] + rowy = [] + rowz = [] + + self.rad_scene = self.points_3D + vel = self.points_3D[:,3] + mtl = self.points_3D[:,4] + + pcd = o3d.geometry.PointCloud() + pcd.points = o3d.utility.Vector3dVector(self.rad_scene[:, 0:3]) + # o3d.visualization.draw_geometries([pcd]) + + + diameter = np.linalg.norm( + np.asarray(pcd.get_max_bound()) - np.asarray(pcd.get_min_bound())) + + print("Define parameters used for hidden_point_removal",diameter) + camera = [radar.center[0], radar.center[1], radar.elv] + radius = 10000 + # pdb.set_trace() + print("Get all points that are visible from given view point") + try: + _, pt_map = pcd.hidden_point_removal(camera, radius) + except Exception as e: + pt_map = np.arange(self.points_3D.shape[0]) + print(e) + + + self.rad_scene = np.hstack((np.array(pcd.points)[pt_map],vel[pt_map,None],mtl[pt_map,None])) + + print(f"Number of points in point cloud = {self.rad_scene.shape[0]}") + vel = self.rad_scene[:,3] + mtl = self.rad_scene[:,4] + pcd = o3d.geometry.PointCloud() + pcd.points = o3d.utility.Vector3dVector(self.rad_scene[:, 0:3]) + pcd_tree = o3d.geometry.KDTreeFlann(pcd) + downpcd = pcd.voxel_down_sample(voxel_size=0.05) + valid_points= np.zeros((1,5)) + for point in range(0,len(np.asarray(downpcd.points))): + [_, idx,_] = pcd_tree.search_knn_vector_3d(downpcd.points[point], 1) + valid_point = np.concatenate((np.array(pcd.points)[idx].T,vel[idx,None],mtl[idx,None]),axis=0).flatten() + valid_points = np.concatenate((valid_points,[valid_point]),axis=0) + valid_points=np.delete(valid_points,0,axis=0) + self.rad_scene = valid_points + print(f"Number of points after downsampling = {self.rad_scene.shape[0]}") + + # print(f"Number of points in point cloud = {self.rad_scene.shape[0]}") + # # downsample_ind = np.random.choice(self.rad_scene.shape[0], round(0.25*self.rad_scene.shape[0])) + # try: + # downsample_ind = np.random.choice(self.rad_scene.shape[0], 4000) ## need to fix this by breaking the number of points in heatmapgen + # self.rad_scene = self.rad_scene[downsample_ind,:] + # except Exception as e: + # pass + + + + # print(f"Number of points after downsampling = {self.rad_scene.shape[0]}") + + # pcd2 = o3d.geometry.PointCloud() + # pcd2.points = o3d.utility.Vector3dVector(self.rad_scene[:, 0:3]) + # o3d.visualization.draw_geometries([pcd2]) + # sys.exit() + + def specularpoints(self, radar): + + pcd = o3d.geometry.PointCloud() + pcd.points = o3d.utility.Vector3dVector(self.rad_scene[:, 0:3]) + + pcd.estimate_normals( search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=10, max_nn=10)) + # o3d.visualization.draw_geometries([pcd]) + + # plt.figure() + # pcd = o3d.geometry.PointCloud() + # pcd.points = o3d.utility.Vector3dVector(self.rad_scene) + # o3d.visualization.draw_geometries([pcd]) + # plt.title('Estimated Normals of Point Cloud') + + x = np.array(pcd.points)[:, 0] + y = np.array(pcd.points)[:, 1] + z = np.array(pcd.points)[:, 2] + u = np.array(pcd.normals)[:, 0] + v = np.array(pcd.normals)[:, 1] + w = np.array(pcd.normals)[:, 2] + + # fig = plt.figure() + # ax = fig.gca(projection='3d') + # ax.quiver(x, y, z, u, v, w) + # plt.show() + + angles = np.arccos(np.sum(np.array(pcd.normals)[:, 0: 3]*((np.array([x, y, z])-np.hstack((radar.center, radar.elv)).reshape((3, 1))).T /np.linalg.norm(np.array([x, y, z])-np.hstack((radar.center, radar.elv)).reshape((3, 1)), axis=0)[:, np.newaxis]), axis=1)) + angles_carla = self.rad_scene[:, 5] + spec_mask = self.rad_scene + + rho = np.linalg.norm(spec_mask[:, 0:3] - np.hstack((radar.center, radar.elv)), axis=1) + theta = math.pi / 2 - np.arctan(((spec_mask[:, 0] - radar.center[0]) / (spec_mask[:, 1] - radar.center[1]))) + elev_angle = np.arccos((spec_mask[:, 2] - radar.elv)/rho) + # loss = np.zeros((len(theta),), dtype=float) + # for i in range(len(theta)): + # if self.rad_scene[i, 4] == 0.1: + # loss[i] = (0.02 * (((1 + np.abs(np.cos(angles[i]))) / 2) ** 0.01) + 0.15*np.abs(np.cos(angles[i]))**300) * self.rad_scene[i, 4] * rho[i] * rho[i] * 3282 * math.radians(radar.voxel_phi) * math.radians(radar.voxel_theta) # + 0.1*np.abs(np.cos(angles))**300 + # else: # after metal without concrete 0.15,300 # after concrete 0.6,600 (just for the else loss) + # loss[i] = (0.02 * (((1 + np.abs(np.cos(angles[i]))) / 2) ** 0.01) + 0.6*np.abs(np.cos(angles[i]))**600) * self.rad_scene[i, 4] * rho[i] * rho[i] * 3282 * math.radians(radar.voxel_phi) * math.radians(radar.voxel_theta) # radar.voxel_phi * radar.voxel_theta + speed = self.rad_scene[:, 3] # 0.05&5 , 0.1&300 + + + + # file_path = os.path.join('F:/28_07_data/28_07_lidar_data/test_28_7_run11/mtlb-pcdFromBag', '0001.pcd') + # pc = o3d.io.read_point_cloud(file_path) + # pc.points = o3d.utility.Vector3dVector(self.rad_scene[:, 0:3]) + # o3d.visualization.draw_geometries([pc]) + loss_att,_,_ = get_loss_3(self.rad_scene, rho, elev_angle , angles_carla, radar, use_spec = False, use_diffused = True, no_material=False) + + return rho, theta, loss_att, speed, angles + + +def get_loss(points, rho, angles, radar, use_spec = True, use_diffused = True, no_material = False): + + # loss = np.zeros((len(theta),), dtype=float) + + if no_material: + points[:,4] = 1 + + constant = points[:, 4] * rho * rho * 3282 * math.radians(radar.voxel_phi) * math.radians(radar.voxel_theta) # N-points length + + diff_scatter = 0.02 * (((1 + np.abs(np.cos(angles))) / 2) ** 0.01) # N-points length + + spec_gain = 0.6 * np.ones(len(angles),) + spec_exp = 600 * np.ones(len(angles),) + + #for metal + spec_gain[points[:, 4] == 0.1] = 0.15 + spec_exp[points[:, 4] == 0.1] = 300 + + specular = spec_gain*np.abs(np.cos(angles))**spec_exp + + loss = (use_diffused * diff_scatter + use_spec * specular) * constant + + return loss, diff_scatter*constant, specular*constant + + +def get_loss_2(points, rho, angles, radar, use_spec = True, use_diffused = True, no_material = False): + ''' + Note: angles should be used for power calculation as it is in reflecting surface coordinate system while theta is in radar coordinate system + + Implemement power conservation here: + P_inc = P_return + P_trans + P_inc = P_inc * T^2 + P_inc * (1 - T^2) + P_inc = P_inc * T^2 (R^2 + S^2) + P_trans + + Assume P_trans=0 or T=1. This can be more accurately calculated from fresnel equations + + P_inc = (P_inc * R^2) + (P_inc * S^2) + P_inc = P_spec + P_scat + ''' + + if no_material: + points[:,4] = 1 + + + ##Assuming rho and theta are calculated correctly + P_spec_inc = np.zeros((len(angles),), dtype=float) + P_scat_inc = np.zeros((len(angles),), dtype=float) + + K_sq = 3282 # Gt Pt + voxel_theta = np.deg2rad(radar.voxel_theta) + voxel_phi = np.deg2rad(radar.voxel_phi) + + tx_dist_loss_exponent = 0 + rx_dist_loss_exponent = 0 + + P_incident = np.power(rho,2) * np.sin(voxel_theta) * voxel_phi * voxel_theta * (1/np.power(rho,tx_dist_loss_exponent)) * K_sq # rho.^1.4 + + + + #Material dependent qualtities + if use_spec and use_diffused: + R_sq = np.ones((len(angles),))*0.8 # metal 0.3 + R_sq[points[:,4]==0.03] = 0.2 # Concrete + R_sq[points[:,4]==0.001] = 0.2 # Vegetation + R_sq[points[:,4]==0] = 0.1 # Road + S_sq = 1 - R_sq # Scattering coefficient + + elif not use_spec: + R_sq = np.zeros((len(angles),)) + S_sq = 1 - R_sq # Scattering coefficient + + elif not use_diffused: + R_sq = np.ones((len(angles),)) + S_sq = 1 - R_sq # Scattering coefficient + + else: + print("Both Scatter and specular can't be false") + AssertionError + + P_absorbed_fac = np.zeros((len(angles),)) # metal + P_absorbed_fac[points[:,4]==0.03] = 0.8 # concrete + P_absorbed_fac[points[:,4]==0.001] = 0.8 # vegetation + P_absorbed_fac[points[:,4]==0] = 1 # road + + + P_reflected = P_incident*(1 - P_absorbed_fac) + + spec_exp = np.ones((len(angles),))*600 + spec_exp[points[:,4]==0.1] = 200 #300 # metal + + normalization = np.ones((len(angles),))*(1/0.10229) + normalization[points[:,4]==0.1] = (1/0.17702) # metal + + + + P_spec = P_reflected * R_sq + P_scat = P_reflected * S_sq + + ##find power in the incident direction now + # pdb.set_trace() + # P_spec_inc[points[:,4]==0.1] = points[points[:,4]==0.1,4]*0.15*np.abs(np.cos(angles[points[:,4]==0.1]))**300 + # P_spec_inc[points[:,4]!=0.1] = points[points[:,4]!=0.1,4]*0.6*np.abs(np.cos(angles[points[:,4]!=0.1]))**600 + + P_spec_inc = normalization*(np.abs(np.cos(angles))**spec_exp)*(1/np.power(rho,rx_dist_loss_exponent)) + # P_spec_inc[points[:,4]!=0.1] = (1/0.10229)*(np.abs(np.cos(angles[points[:,4]==0.1]))**600)*(1/np.power(rho,2)) + + P_spec_inc = P_spec_inc * P_spec + + #Lambertian + P_scat_inc = P_scat* ((0.5*(1+abs(np.cos(angles))))**0.1)*(1/3.07385) * (1/np.power(rho,rx_dist_loss_exponent)) # lambda sqaure aperture taken care in heatmap + + P_spec_inc = P_spec_inc * 10e2 #30e2 #0.8e2 + P_scat_inc = P_scat_inc * 10e2 #30e2 + + loss = P_spec_inc + P_scat_inc + + return loss, P_scat_inc, P_spec_inc + +def get_loss_3(points, rho, elev_angle, angles, radar, use_spec = True, use_diffused = True, no_material = False): + ''' + Note: angles should be used for power calculation as it is in reflecting surface coordinate system while theta is in radar coordinate system + + Implemement power conservation here: + P_inc = P_return(rough_loss^2 + S^2) + P_trans + P_scatter = P_return*S^2 + P_return = P_inc * R^2 (R -> reflection co-eff from fresnel equations) + + S = sqrt(1 - rough_loss^2) + + + R_TE = cos(incd_angle) - sqrt(perm - sin(incd_angle)^2) / cos(incd_angle) + sqrt(perm - sin(incd_angle)^2) + R_TM = perm*cos(incd_angle) - sqrt(perm - sin(incd_angle)^2) / perm*cos(incd_angle) + sqrt(perm - sin(incd_angle)^2) + R^2 = (R_TE^2 + R_TM^2)/2 + + Scatter_loss = R^2 * S^2 = R^2 * (1- rough_loss^2) + rough_loss = exp(-0.5((4*pi*std_rough*cos(inc_angle)/lambda)^2)) + + backscatter_lobe_loss = E_so^2 * (lobe_frac*cos(incd_angle)^2*alpha_R + (1-lobe_frac)) + ''' + + # standard deviation of surface roughness + unlabel_roughness = 0 + wood_roughness = 0.0017 #1.7mm + conc_roughness = 0.0017 #1.7mm + human_roughness = 0.01 # 100um + metal_roughness = 0.00005 # 100um + + roughness = np.array([unlabel_roughness,wood_roughness,conc_roughness,human_roughness,metal_roughness]) + + unlabel_perm = 1 + wood_perm = 2 + conc_perm = 5.24 + human_perm = 1#15 + metal_perm = 100000 + + permittivity = np.array([unlabel_perm,wood_perm,conc_perm,human_perm,metal_perm]) + + scat_normalization = 1/1.09*np.ones((len(angles),)) #*(1/0.10229) #####?????????#### this should bring the Eso^2 #####?????????#### + lobe_frac = 0.9 # ratio of energy in main_lobe/back_scatter_lobe + spec_lobe_exp = 4.0 #lobe exponential along specular direction + + K_sq = 3282 # Gt Pt #####?????????#### is this from the peak amplitude calculation #####?????????#### + voxel_theta = np.deg2rad(radar.voxel_theta) + voxel_phi = np.deg2rad(radar.voxel_phi) + + tx_dist_loss_exponent = 2 + rx_dist_loss_exponent = 0 + + spec_angle_thresh = 2*np.pi/180#*(1/rho) + + P_incident = np.power(rho,2) * np.sin(elev_angle) * voxel_phi * voxel_theta * (1/np.power(rho,tx_dist_loss_exponent)) * K_sq + + material = np.array(points[:,4]) + material = np.asarray(material, dtype = 'int') + # material[:] = 3 + + #Material dependent qualtities + # if use_spec and use_diffused: + R_TE_sq = np.power(((abs(np.cos(angles)) - np.sqrt(permittivity[material] - np.power(np.sin(angles),2)))/(abs(np.cos(angles)) + np.sqrt(permittivity[material] - np.power(np.sin(angles),2)))),2) + R_TM_sq = np.power(((permittivity[material]*abs(np.cos(angles)) - np.sqrt(permittivity[material] - np.power(np.sin(angles),2)))/(permittivity[material]*abs(np.cos(angles)) + np.sqrt(permittivity[material] - np.power(np.sin(angles),2)))),2) + R_sq = R_TE_sq #0.5 * (R_TE_sq + R_TM_sq) + rough_sq = np.exp(-0.5*np.power(4*np.pi*roughness[material]*abs(np.cos(angles))*radar.f/radar.c,2)) + # if len(rough_sq[abs(angles) < spec_angle_thresh])!=0: + # print(f"Materials = {np.unique(material)}") + # print(f"R value max= {np.max(rough_sq[abs(angles) < spec_angle_thresh])}; average = {np.mean(rough_sq[abs(angles) < spec_angle_thresh])}") + + S_sq = 1 - rough_sq # Scattering coefficient = P_scat/P_reflec + P_reflected = P_incident * R_sq + P_scat = P_reflected * S_sq + P_scat_lobe = scat_normalization*(lobe_frac * np.power(abs(np.cos(angles)),2*spec_lobe_exp) + (1-lobe_frac)) + P_scat = P_scat * np.power(P_scat_lobe,2) #lobe shape is for amplitude, need to square for power + + P_spec = P_reflected * rough_sq + P_spec = P_spec * np.ones((len(angles),)) * (abs(angles) < spec_angle_thresh) # specular reflection only if angle < 2 degrees + + if not use_spec: + # R_TE_sq = np.power(((abs(np.cos(angles)) - np.sqrt(permittivity[material] - np.power(np.sin(angles),2)))/(abs(np.cos(angles)) + np.sqrt(permittivity[material] - np.power(np.sin(angles),2)))),2) + # R_TM_sq = np.power(((permittivity[material]*abs(np.cos(angles)) - np.sqrt(permittivity[material] - np.power(np.sin(angles),2)))/(permittivity[material]*abs(np.cos(angles)) + np.sqrt(permittivity[material] - np.power(np.sin(angles),2)))),2) + # R_sq = R_TE_sq #0.5 * (R_TE_sq + R_TM_sq) + # rough_sq = np.exp(-0.5*np.power(4*np.pi*roughness[material]*abs(np.cos(angles))*radar.f/radar.c,2)) + + # S_sq = 1 - rough_sq # Scattering coefficient = P_scat/P_reflec + # P_reflected = P_incident * R_sq + # P_scat = P_reflected * S_sq + # P_scat_lobe = scat_normalization*(lobe_frac * np.power(abs(np.cos(angles)),2*spec_lobe_exp) + (1-lobe_frac)) + # P_scat = P_scat * np.power(P_scat_lobe,2) #lobe shape is for amplitude, need to square for power + + P_spec = np.zeros((len(angles),)) + + elif not use_diffused: + # R_TE_sq = np.power(((abs(np.cos(angles)) - np.sqrt(permittivity[material] - np.power(np.sin(angles),2)))/(abs(np.cos(angles)) + np.sqrt(permittivity[material] - np.power(np.sin(angles),2)))),2) + # R_TM_sq = np.power(((permittivity[material]*abs(np.cos(angles)) - np.sqrt(permittivity[material] - np.power(np.sin(angles),2)))/(permittivity[material]*abs(np.cos(angles)) + np.sqrt(permittivity[material] - np.power(np.sin(angles),2)))),2) + # R_sq = R_TE_sq #0.5 * (R_TE_sq + R_TM_sq) + # rough_sq = np.exp(-0.5*np.power(4*np.pi*roughness[material]*abs(np.cos(angles))*radar.f/radar.c,2)) + + # S_sq = 1 - rough_sq # Scattering coefficient = P_scat/P_reflec + # P_reflected = P_incident * R_sq + # P_scat = P_reflected * S_sq + # P_scat_lobe = scat_normalization*(lobe_frac * np.power(abs(np.cos(angles)),2*spec_lobe_exp) + (1-lobe_frac)) + # P_scat = P_scat * np.power(P_scat_lobe,2) #lobe shape is for amplitude, need to square for power + + P_scat = np.zeros((len(angles),)) + + # P_spec = P_reflected * rough_sq + # P_spec = P_spec * np.ones((len(angles),)) * (abs(angles) < 2*np.pi/180) #####?????????#### specular reflection only if angle < 2 degrees #####?????????#### + + elif not use_spec and not use_diffused: + print("Both Scatter and specular can't be false") + AssertionError + + P_spec = P_spec *4*100*25/9 + P_scat = P_scat *100*25/9 + + loss = P_spec + P_scat + # loss = loss * 2**7.2 + + # fig, ax = plt.subplots(1,7, figsize=(10,5)) + # ax[0].plot(loss) + # ax[0].set_title("total loss",fontsize=10) + # ax[1].plot(P_spec) + # ax[1].set_title("specular loss",fontsize=10) + # ax[2].plot(P_scat) + # ax[2].set_title("scatter loss",fontsize=10) + # ax[3].plot(material) + # ax[3].set_title("material",fontsize=10) + # ax[4].plot(angles) + # ax[4].set_title("angles",fontsize=10) + # ax[5].plot(R_TE_sq) + # ax[5].set_title("R TE",fontsize=10) + # ax[6].plot(R_TM_sq) #abs(numerator)/abs(denominator)) + # ax[6].set_title("R TM",fontsize=10) + + # plt.show() + # plt.suptitle('loss profile',fontsize=15, y=1) + # plt.savefig('loss_comp_sim.jpg') + + return loss, P_scat, P_spec + + +if __name__ == '__main__': + + # dummy_points = np.array([[0,0,0,0,0.1], # + # [0,0,0,0,0.03], + # [0,0,0,0,0.006]]) + + + dummy_points = np.vstack((np.tile([0,0,0,0,0.1],(180,1)),np.tile([0,0,0,0,0.03],(180,1)),np.tile([0,0,0,0,0.001],(180,1)))) + latest_dummy_points = np.vstack((np.tile([0,0,0,0,4],(180,1)),np.tile([0,0,0,0,2],(180,1)),np.tile([0,0,0,0,1],(180,1)))) + + dummy_rho = np.array([20]*540) + + dummy_angles = np.hstack((np.linspace(-np.pi/2,np.pi/2,180),np.linspace(-np.pi/2,np.pi/2,180),np.linspace(-np.pi/2,np.pi/2,180))) + + radar = radar() + + #loss_old, scat_old, spec_old = get_loss(dummy_points,dummy_rho,dummy_angles,radar) + loss_new, scat_new, spec_new = get_loss_2(dummy_points,dummy_rho,dummy_angles,radar) + loss_latest, scat_latest, spec_latest = get_loss_3(latest_dummy_points,dummy_rho,dummy_angles,radar) + + + # print(f"Old Loss: {loss_old}") + # print(f"New Loss: {loss_new}") + + fig, ax = plt.subplots(2,3, figsize=(10,5)) + + #ax[0,0].plot(loss_old) + #ax[0,1].plot(spec_old) + #ax[0,2].plot(scat_old) + ax[0,0].plot(loss_latest) + ax[0,0].set_title("New total loss",fontsize=10) + ax[0,1].plot(spec_latest) + ax[0,1].set_title("New specular loss",fontsize=10) + ax[0,2].plot(scat_latest) + ax[0,2].set_title("New scatter loss",fontsize=10) + ax[1,0].plot(loss_new) + ax[1,0].set_title("Prev total loss",fontsize=10) + ax[1,1].plot(spec_new) + ax[1,1].set_title("Prev specular loss",fontsize=10) + ax[1,2].plot(scat_new) + ax[1,2].set_title("Prev scatter loss",fontsize=10) + + plt.show() + plt.suptitle('0:179 => Metal 180:359 =>Concrete 360:539=>Wood',fontsize=15, y=1) + plt.savefig('loss_comparison.jpg') + diff --git a/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/shenron/heatmap_gen.py b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/shenron/heatmap_gen.py new file mode 100644 index 0000000..32252a2 --- /dev/null +++ b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/shenron/heatmap_gen.py @@ -0,0 +1,177 @@ +import numpy as np +from e2e_agent_sem_lidar2shenron_package.ConfigureRadar import radar +import matplotlib.pyplot as plt +from matplotlib import cm +from mat4py import loadmat +import math +from mpl_toolkits.mplot3d import axes3d, Axes3D +import time +import scipy.io as sio +from numba import jit +from numba import int32, float64, complex128 +from noise_utils import get_noise +import pdb + +# @jit(nopython=True) +def heatmap_gen(rho, theta, loss, speed, radar, plot_fig, return_power): + + range_res = radar.c / (2 * radar.B) + max_range = range_res * radar.N_sample + + Ts = 1 / radar.samp_rate + t = np.arange(0, radar.chirpT, Ts) + tau_resolution = 1 / radar.B + k = radar.B / radar.chirpT + x = np.exp(1j * 2 * np.pi * (radar.f + 0.5 * k * t) * t) + + # noise_prop = loadmat('noise.mat') + # real_fft_ns = np.random.normal(noise_prop['noise_mean_real'], noise_prop['noise_std_real']).T + # complex_fft_ns = np.random.normal(noise_prop['noise_mean_real'], noise_prop['noise_std_real']).T + # final_noise = real_fft_ns + 1j * complex_fft_ns + # signal_Noisy = np.fft.ifft(final_noise, radar.N_sample, 1) * 10 + signal_Noisy = get_noise(radar) + + if radar.doppler_mode: + measurement = np.zeros((radar.chirps, radar.nRx, radar.N_sample), dtype="complex128") # doppler,AoA,range + # start = time.time() + for chirp in range(radar.chirps): + tau = 2 * rho / radar.c + chirp * radar.chirpT * speed / radar.c + _lambda = radar.c / radar.f + sRx = _lambda / 2 + _lambda = radar.c / radar.f + tau_vec = np.zeros((radar.nRx, len(rho))) + for i in range(radar.nRx): + tau_vec[i, :] = tau + i * sRx * np.sin(np.pi / 2 - theta) / radar.c + + sum_samp = np.zeros((radar.nRx, radar.N_sample), dtype="complex128") + for j in range(rho.shape[0]): + if return_power: + if rho[j] != 0: + signal = np.sqrt(loss[j]) * np.exp( + 1j * 2 * np.pi * (radar.f + 0.5 * k * (np.expand_dims(t,0) - np.expand_dims(tau_vec[:,j],1))) * ( + np.expand_dims(t,0) - np.expand_dims(tau_vec[:,j],1))) + else: + if rho[j] != 0: + signal = loss[j] * (1 / rho[j] ** 2) * np.exp( + 1j * 2 * np.pi * (radar.f + 0.5 * k * (np.expand_dims(t,0) - np.expand_dims(tau_vec[:,j],1))) * ( + np.expand_dims(t,0) - np.expand_dims(tau_vec[:,j],1))) + +# noise_real = (1j*1j*-1) * (2 * np.random.rand(radar.nRx, radar.N_sample) - 1) +# noise_complex = (1j) * (2 * np.random.rand(radar.nRx, radar.N_sample) - 1) +# noise = (noise_real+noise_complex) * radar.noise_amp +# signal_Noisy = signal + noise + + sum_samp = sum_samp + signal + adc_sampled = np.sqrt(radar.gain * _lambda ** 2 / (4 * np.pi) ** 3) * np.conj(sum_samp) * (x) + adc_sampled = adc_sampled + signal_Noisy + measurement[chirp, :, :] = adc_sampled + + return measurement + # end = time.time() + # diction = {"doppler_cube": measurement} + # sio.savemat("E:/Radar_sim/simlator/git repo/Heatmap-sim/doppler_cube.mat", diction) +# RangeFFT = np.fft.fft(measurement, radar.N_sample, 2) +# AngleFFT = np.fft.fftshift(np.fft.fft(RangeFFT[0, :, :], radar.angle_fft_size, 0), 0) +# Doppler_data = np.fft.fftshift(np.fft.fft(np.fft.fftshift(np.fft.fft(RangeFFT, radar.angle_fft_size, 1), 1), +# radar.chirps, 0), 0) +# Doppler_heatmap = np.sum(np.arange(radar.chirps)[:, None, None] * np.abs(Doppler_data), axis=0) / np.sum( +# np.abs(Doppler_data), axis=0) - radar.chirps / 2 + + else: + tau = 2 * rho / radar.c + _lambda = radar.c / radar.f + sRx = _lambda / 2 # separation + _lambda = radar.c / radar.f + + tau_vec = np.zeros((radar.nRx, rho.shape[0])) + for i in range(radar.nRx): + tau_vec[i, :] = tau + (i) * sRx * np.sin(np.pi / 2 - theta) / radar.c + + sum_samp = np.zeros((radar.nRx, radar.N_sample), dtype="complex128") + for j in range(rho.shape[0]): + if (rho[j] != 0): + if return_power: + if rho[j] != 0: + signal = 10**10 * np.sqrt(loss[j]) * np.exp( + 1j * 2 * np.pi * (radar.f + 0.5 * k * (np.expand_dims(t,0) - np.expand_dims(tau_vec[:,j],1))) * ( + np.expand_dims(t,0) - np.expand_dims(tau_vec[:,j],1))) + else: + if rho[j] != 0: + signal = loss[j] * (1 / rho[j] ** 2) * np.exp( + 1j * 2 * np.pi * (radar.f + 0.5 * k * (np.expand_dims(t,0) - np.expand_dims(tau_vec[:,j],1))) * ( + np.expand_dims(t,0) - np.expand_dims(tau_vec[:,j],1))) + +# noise_real = (1j*1j*-1) * (2 * np.random.rand(radar.nRx, radar.N_sample) - 1) +# noise_complex = (1j) * (2 * np.random.rand(radar.nRx, radar.N_sample) - 1) +# noise = (noise_real+noise_complex) * radar.noise_amp +# signal_Noisy = signal + noise + + sum_samp = sum_samp + signal + + adc_sampled = np.sqrt(radar.gain * _lambda ** 2 / (4 * np.pi) ** 3) * np.conj(sum_samp) * (x) + adc_sampled = adc_sampled + signal_Noisy + # RangeFFT = np.fft.fft(adc_sampled, radar.N_sample, 1) + + # pwr_prof = 10*np.log10(np.sum(abs(RangeFFT)**2, 0)+1) + # plt.plot(radar.range_res*np.arange(radar.N_sample), pwr_prof) + # plt.axis([0, 256*radar.range_res, 70, 180]) + # plt.show() + + # AngleFFT = np.fft.fftshift(np.fft.fft(RangeFFT, radar.angle_fft_size, 0), 0) + + # Doppler_heatmap = np.zeros(np.shape(AngleFFT)) + + return measurement + + + # print('runtime is ', end - start) + +# d = 1 +# sine_theta = -2 * np.arange(-radar.angle_fft_size / 2, (radar.angle_fft_size / 2) + 1) / radar.angle_fft_size / d +# # sine_theta = -2*np.arange(-radar.angle_fft_size/2,(radar.angle_fft_size/2)+1)/radar.angle_fft_size/d +# cos_theta = np.sqrt(1 - sine_theta ** 2) +# indices_1D = np.arange(0, radar.N_sample) +# [R_mat, sine_theta_mat] = np.meshgrid(indices_1D * range_res, sine_theta) +# [_, cos_theta_mat] = np.meshgrid(indices_1D, cos_theta) + +# x_axis = R_mat * cos_theta_mat +# y_axis = R_mat * sine_theta_mat +# mag_data_static = abs(np.vstack( +# (AngleFFT, AngleFFT[255, :]))) # np.column_stack((abs(AngleFFT[indices_1D,:]),abs(AngleFFT[indices_1D,0]))) +# mag_data_doppler = abs(np.vstack((Doppler_heatmap, Doppler_heatmap[255, :]))) +# # doppler_cube = np.concatenate((Doppler_data, Doppler_data[:, 255, :][:, np.newaxis, :]), 1) + +# mag_data_static = np.flipud(mag_data_static) +# mag_data_doppler = np.flipud(mag_data_doppler) +# # doppler_cube = np.flipud(doppler_cube) + +# return x_axis, y_axis, mag_data_static, mag_data_doppler + + +if __name__ == '__main__': + radar = radar() + + test_WI=1 + if (test_WI): + points = np.load("E:/Radar_sim/simlator/git repo/Heatmap-sim/simulation5.npy") + rho = np.linalg.norm(points[:, 0:3] - np.array(radar.center+radar.elv), axis=1) + theta = math.pi / 2 - np.arctan(((points[:, 0] - radar.center[0]) / (points[:, 1] - radar.center[1]))) + loss = points[:, 3] + speed = np.zeros_like(rho) + else: + rho = np.array([5, 5, 10]) # np.linspace(7.9749, 9.2793, 97) + theta = np.array([math.pi / 3, math.pi * 2 / 3, math.pi / 2]) # np.linspace(1.6856, 1.521, 97) + loss = np.ones_like(rho) + speed = np.zeros_like(rho) + + x_axis, y_axis, plot_data, doppler_data = heatmap_gen(rho, theta, loss, speed, radar, 1) + + diction = {"x_axis": x_axis, "y_axis": y_axis, "plot_data": plot_data} + sio.savemat('test_heatmap.mat', diction) + + # fig, ax = plt.subplots(subplot_kw={"projection": "3d"}) + # ax.view_init(elev=90, azim=180) + # surf = ax.plot_surface(x_axis, y_axis, plot_data ** 0.7, cmap=cm.coolwarm, + # linewidth=0, antialiased=False) + # plt.show() + diff --git a/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/shenron/heatmap_gen_fast.py b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/shenron/heatmap_gen_fast.py new file mode 100644 index 0000000..eac4e02 --- /dev/null +++ b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/shenron/heatmap_gen_fast.py @@ -0,0 +1,280 @@ +import numpy as np +from e2e_agent_sem_lidar2shenron_package.ConfigureRadar import radar +import matplotlib.pyplot as plt +from matplotlib import cm +from mat4py import loadmat +import math +from mpl_toolkits.mplot3d import axes3d, Axes3D +import time +import scipy.io as sio +# from numba import jit +# from numba import int32, float64, complex128 +import pdb +import torch +from pynvml import * + +def get_gpu_id_most_avlbl_mem(): + try: + nvmlInit() + deviceCount = nvmlDeviceGetCount() + free = [] + for i in range(deviceCount): + h = nvmlDeviceGetHandleByIndex(i) + info = nvmlDeviceGetMemoryInfo(h) + free.append(info.free) + free = np.array(free) + # h0 = nvmlDeviceGetHandleByIndex(0) + # h1 = nvmlDeviceGetHandleByIndex(1) + # h2 = nvmlDeviceGetHandleByIndex(2) + # h3 = nvmlDeviceGetHandleByIndex(3) + # info0 = nvmlDeviceGetMemoryInfo(h0) + # info1 = nvmlDeviceGetMemoryInfo(h1) + # info2 = nvmlDeviceGetMemoryInfo(h2) + # info3 = nvmlDeviceGetMemoryInfo(h3) + + # free = np.array([info0.free,info1.free,info2.free,info3.free]) + + return np.argmax(free), np.max(free) + except Exception: + return 0, 0 + +# @jit(nopython=True) +def heatmap_gen(rho, theta, loss, speed, radar, plot_fig, return_power): + start = time.time() + range_res = radar.c / (2 * radar.B) + max_range = range_res * radar.N_sample + + Ts = 1 / radar.samp_rate + t = np.arange(0, radar.chirpT, Ts) + tau_resolution = 1 / radar.B + k = radar.B / radar.chirpT + x = np.exp(1j * 2 * np.pi * (radar.f + 0.5 * k * t) * t) + + + _lambda = radar.c / radar.f + sRx = _lambda / 2 + _lambda = radar.c / radar.f + + # Declare if we should use cpu or cuda + try: + if torch.cuda.is_available(): + gpu_id, _ = get_gpu_id_most_avlbl_mem() + device = torch.device(f'cuda:{gpu_id}') + else: + gpu_id = 0 + device = torch.device('cpu') + except Exception: + gpu_id = 0 + device = torch.device('cpu') + + print(f"------- Using {device}:{gpu_id} -------") + + # beamforming_vector_constant = np.zeros((radar.nRx,rho.shape[0])) + delta = torch.zeros((radar.nRx,rho.shape[0]),device= device) + + + ##Noise + # noise_prop = loadmat('noise.mat') + # real_fft_ns = np.random.normal(noise_prop['noise_mean_real'], noise_prop['noise_std_real']).T + # complex_fft_ns = np.random.normal(noise_prop['noise_mean_real'], noise_prop['noise_std_real']).T + # final_noise = real_fft_ns + 1j * complex_fft_ns + # signal_Noisy = np.fft.ifft(final_noise, radar.N_sample, 1) * 10 + signal_Noisy = radar.get_noise() + + ##initialising on torch + loss = torch.tensor(loss*(1 / rho ** 2), device=device).float() + rho = torch.tensor(rho, device=device).float() + signal_Noisy = torch.tensor(signal_Noisy, device=device) + theta = torch.tensor(theta, device=device).float() + speed = torch.tensor(speed, device=device).float() + t = torch.tensor(t, device=device).float() + + torch.set_printoptions(precision=10) + if radar.doppler_mode: + measurement = np.zeros((radar.chirps, radar.nRx, radar.N_sample), dtype="complex128") # doppler,AoA,range + # start = time.time() + for chirp in range(radar.chirps): + tau = 2 * (rho / radar.c + chirp * (radar.chirp_rep) * speed / radar.c) + # tau = 2 * rho / radar.c + chirp * (12*27e-6) * speed / radar.c + # print(tau[torch.argmax(abs(speed))]) + for i in range(radar.nRx): + delta[i,:] = i * sRx * torch.sin(np.pi / 2 - theta) / radar.c + + # delta = torch.tensor(delta,device='cuda') + # tau = torch.tensor(tau,device='cuda') + # t = torch.tensor(t,device='cuda') + + # beamforming_vector = np.exp(1j*2*np.pi*(radar.f*delta[:,:,None] - 0.5*k*delta[:,:,None]**2 - k*tau[None,:,None]*delta[:,:,None] + k*delta[:,:,None]@t[None,None,:])) #(80xN) + beamforming_vector = torch.exp(1j*2*np.pi*(radar.f*delta[:,:,None] - 0.5*k*delta[:,:,None]**2 - k*tau[None,:,None]*delta[:,:,None] + k*delta[:,:,None]@t[None,None,:])) #(80xN) + + + # tau = tau.cpu().numpy() + # t = t.cpu().numpy() + # beamforming_vector = beamforming_vector.cpu().numpy() + + + dechirped = torch.exp((1j * 2 * np.pi) *(radar.f * tau[:,None] + k * tau[:,None]@t[None,:] - 0.5 * k * tau[:,None]**2)) + # loss_factor = (loss * (1 / rho ** 2))[:,None] ## from network + loss_factor = (torch.sqrt(loss)[:,None]) ## from network + + signal_single_antenna = loss_factor*dechirped # (Nx256) + + signal = beamforming_vector*signal_single_antenna[None,:,:] # (80x256) + + signal = torch.squeeze(torch.sum(signal,1)) + + # pdb.set_trace() + + # (np.expand_dims(t,0) - np.expand_dims(tau_vec[:,j],1))) * ( + # np.expand_dims(t,0) - np.expand_dims(tau_vec[:,j],1))) + + # signal = np.exp(1j*2*np.pi * ()) + + # noise_real = (1j*1j*-1) * (2 * np.random.rand(radar.nRx, radar.N_sample) - 1) + # noise_complex = (1j) * (2 * np.random.rand(radar.nRx, radar.N_sample) - 1) + # noise = (noise_real+noise_complex) * radar.noise_amp + # signal_Noisy = signal + + # sum_samp = sum_samp + signal_Noisy ## Here I am not adding individual noise for each point + + + + # adc_sampled = np.sqrt(radar.gain * _lambda ** 2 / (4 * np.pi) ** 3) * np.conj(signal_Noisy) * (x) + adc_sampled = torch.sqrt(torch.tensor(radar.gain * _lambda ** 2 / (4 * np.pi) ** 3)) * signal + adc_sampled = adc_sampled + signal_Noisy + + adc_sampled = adc_sampled.cpu().numpy() + + + + measurement[chirp, :, :] = adc_sampled + # pdb.set_trace() + del rho + del loss + # del signal_noisy + del speed + del theta + del t + del dechirped + del beamforming_vector + del signal + del signal_single_antenna + if device.type == 'cuda': + with torch.cuda.device(f'cuda:{gpu_id}'): + torch.cuda.empty_cache() + # pdb.set_trace() + return measurement + # end = time.time() + # diction = {"doppler_cube": measurement} + # sio.savemat("E:/Radar_sim/simlator/git repo/Heatmap-sim/doppler_cube.mat", diction) +# RangeFFT = np.fft.fft(measurement, radar.N_sample, 2) +# AngleFFT = np.fft.fftshift(np.fft.fft(RangeFFT[0, :, :], radar.angle_fft_size, 0), 0) +# Doppler_data = np.fft.fftshift(np.fft.fft(np.fft.fftshift(np.fft.fft(RangeFFT, radar.angle_fft_size, 1), 1), +# radar.chirps, 0), 0) +# Doppler_heatmap = np.sum(np.arange(radar.chirps)[:, None, None] * np.abs(Doppler_data), axis=0) / np.sum( +# np.abs(Doppler_data), axis=0) - radar.chirps / 2 + + else: + tau = 2 * rho / radar.c + _lambda = radar.c / radar.f + sRx = _lambda / 2 # separation + _lambda = radar.c / radar.f + + tau_vec = np.zeros((radar.nRx, rho.shape[0])) + for i in range(radar.nRx): + tau_vec[i, :] = tau + (i) * sRx * np.sin(np.pi / 2 - theta) / radar.c + + sum_samp = np.zeros((radar.nRx, radar.N_sample), dtype="complex128") + for j in range(rho.shape[0]): + if (rho[j] != 0): + if return_power: + if rho[j] != 0: + signal = 10**10 * np.sqrt(loss[j]) * np.exp( + 1j * 2 * np.pi * (radar.f + 0.5 * k * (np.expand_dims(t,0) - np.expand_dims(tau_vec[:,j],1))) * ( + np.expand_dims(t,0) - np.expand_dims(tau_vec[:,j],1))) + else: + if rho[j] != 0: + signal = loss[j] * (1 / rho[j] ** 2) * np.exp( + 1j * 2 * np.pi * (radar.f + 0.5 * k * (np.expand_dims(t,0) - np.expand_dims(tau_vec[:,j],1))) * ( + np.expand_dims(t,0) - np.expand_dims(tau_vec[:,j],1))) + + noise_real = (1j*1j*-1) * (2 * np.random.rand(radar.nRx, radar.N_sample) - 1) + noise_complex = (1j) * (2 * np.random.rand(radar.nRx, radar.N_sample) - 1) + noise = (noise_real+noise_complex) * radar.noise_amp + signal_Noisy = signal + noise + + sum_samp = sum_samp + signal_Noisy*10**5 + + adc_sampled = np.sqrt(radar.gain * _lambda ** 2 / (4 * np.pi) ** 3) * np.conj(sum_samp) * (x) + # RangeFFT = np.fft.fft(adc_sampled, radar.N_sample, 1) + + # pwr_prof = 10*np.log10(np.sum(abs(RangeFFT)**2, 0)+1) + # plt.plot(radar.range_res*np.arange(radar.N_sample), pwr_prof) + # plt.axis([0, 256*radar.range_res, 70, 180]) + # plt.show() + + # AngleFFT = np.fft.fftshift(np.fft.fft(RangeFFT, radar.angle_fft_size, 0), 0) + + # Doppler_heatmap = np.zeros(np.shape(AngleFFT)) + + return measurement + + + # print('runtime is ', end - start) + +# d = 1 +# sine_theta = -2 * np.arange(-radar.angle_fft_size / 2, (radar.angle_fft_size / 2) + 1) / radar.angle_fft_size / d +# # sine_theta = -2*np.arange(-radar.angle_fft_size/2,(radar.angle_fft_size/2)+1)/radar.angle_fft_size/d +# cos_theta = np.sqrt(1 - sine_theta ** 2) +# indices_1D = np.arange(0, radar.N_sample) +# [R_mat, sine_theta_mat] = np.meshgrid(indices_1D * range_res, sine_theta) +# [_, cos_theta_mat] = np.meshgrid(indices_1D, cos_theta) + +# x_axis = R_mat * cos_theta_mat +# y_axis = R_mat * sine_theta_mat +# mag_data_static = abs(np.vstack( +# (AngleFFT, AngleFFT[255, :]))) # np.column_stack((abs(AngleFFT[indices_1D,:]),abs(AngleFFT[indices_1D,0]))) +# mag_data_doppler = abs(np.vstack((Doppler_heatmap, Doppler_heatmap[255, :]))) +# # doppler_cube = np.concatenate((Doppler_data, Doppler_data[:, 255, :][:, np.newaxis, :]), 1) + +# mag_data_static = np.flipud(mag_data_static) +# mag_data_doppler = np.flipud(mag_data_doppler) +# # doppler_cube = np.flipud(doppler_cube) + +# return x_axis, y_axis, mag_data_static, mag_data_doppler + + +if __name__ == '__main__': + radar = radar() + radar.chirps = 1 + radar.center = np.array([0.0, 0.0]) # center of radar + radar.elv = np.array([0.0]) + + test_WI=1 + if (test_WI): + # points = np.load("E:/Radar_sim/simlator/git repo/Heatmap-sim/simulation5.npy") + cell_array = sio.loadmat("wi_data/single_radar_wi_10m_allangles.mat") + + for i in range(36): + print(f"Simulation: {i}") + points = cell_array['cell_array'][i][0] + if points.shape[0]==0: + print("Skipped") + continue + rho = np.linalg.norm(points[:, 0:3], axis=1) + theta = math.pi / 2 - np.arctan(((points[:, 0] - radar.center[0]) / (points[:, 1] - radar.center[1]))) + loss = 10**(points[:, 3]/20) + speed = np.zeros_like(rho) + + + adc_data = heatmap_gen(rho, theta, loss, speed, radar, 1, 0) + diction = {"adc_data": adc_data} + sio.savemat(f"wi_data/simulation_{i}.mat", diction) + + # fig, ax = plt.subplots(subplot_kw={"projection": "3d"}) + # ax.view_init(elev=90, azim=180) + # surf = ax.plot_surface(x_axis, y_axis, plot_data ** 0.7, cmap=cm.coolwarm, + # linewidth=0, antialiased=False) + # plt.show() + + diff --git a/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/simulator_configs.yaml b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/simulator_configs.yaml new file mode 100644 index 0000000..f20f12f --- /dev/null +++ b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/simulator_configs.yaml @@ -0,0 +1,46 @@ +INPUT : "lidar" + +NOISE : True + +BUILDINGS : True + +# RADAR_TYPE : ti_cascade # 1. radarbook | 2. ti_cascade +RADAR_TYPE : ti_cascade + +INDOOR : False + +RADAR_MOVING : False + +RAY_TRACING : False + +INVERT_ANGLE : 0 + +BASE_PATH : "/radar-imaging-dataset/carla_garage_data/ll_dataset_2023_05_10/Routes_Town04_ll_Repetition0/Town04_ll_route0_11_17_05_03_55/" + +CAMERA_PATH : "rgb_image/" #path for original camera images + +LIDAR_PATH : "lidar_pcd/" #path for unprocessed lidar point clouds + +LIDAR_PATH_SEGMENTED : "lidar_segmented/" #path for segmented lidar point clouds + +LIDAR_PATH_CLUSTERS : "lidar_clusters_centroid_with_labels" #path for saving lidar pcd with cluster labels with a separate channel + +LIDAR_PATH_LABELS : "lidar_with_labels/" #path for saving lidar pcd with cluster labels with a separate channel + +LIDAR_PATH_POINT_VELOCITY : "lidar_velocity/" #path for saving the velocity of all the points and all the frames + +LIDAR_PATH_CLUSTER_VELOCITY : "lidar_clusters_centroid_with_label_velocity/" #path for saving the velocity of all the cluster points and all the frames + +RADAR_PATH_SIMULATED : "radar_data_simulated/" + +# CARLA_SHENRON_SEM_LIDAR : carla_shenron_sem_lidar/ +CARLA_SHENRON_SEM_LIDAR : semantic_lidar/ + + +# PATHS : + +# LIDAR_PATH : "C:\\Users\\Strai\\Downloads\\semantic_lidar" + +# LIDAR_FOLDERS : ["semantic_lidar"] + +# OUT_PATH : "C:\\Users\\Strai\\Downloads\\semantic_lidar\\semantic_lidar" \ No newline at end of file diff --git a/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/transform_dsp_utils.py b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/transform_dsp_utils.py new file mode 100644 index 0000000..fdf9715 --- /dev/null +++ b/scripts/ISOLATE/e2e_agent_sem_lidar2shenron_package/transform_dsp_utils.py @@ -0,0 +1,264 @@ +import numpy as np +import matplotlib.pyplot as plt +import torch +import pdb +from scipy import signal +from scipy import io as sio +import matplotlib.pyplot as plt + +def separate_real_imag(input): + + real = input.real + imag = input.imag + + out_tensor = np.stack((real,imag), axis=0) + + return out_tensor + +def separate_mag_phase(input): + + mag = np.abs(input) + phase = np.angle(input) + + out_tensor = np.stack((mag,phase), axis=0) + + return out_tensor + +def convert_adc_to_3d_fft(adcData, rangefft_size = 256, dopplerfft_size=128, anglefft_size = 256, window=False, distance_null = False): + """ + Function to convert adc data to RA tensor + Input: + adcData: + NxDxM -- where N is the range dimension and M is the angle dimension and D is doppler. + + Output: + RA tensor: R-size x A-size + """ + if window: + range_hanning = np.hanning(rangefft_size)[:,None,None] + range_hanning = np.tile(range_hanning, (1,adcData.shape[1],adcData.shape[2])) + + adcData = adcData*range_hanning + + if distance_null: + radar_range_res = 3e8/(2*2.56e8) + range_vector = np.arange(rangefft_size)*radar_range_res + range_vector_loss = range_vector**4 + range_vector_tiled = np.tile(range_vector_loss[:,None, None], (1,adcData.shape[1],adcData.shape[2])) + + adcData = adcData*range_vector_tiled + rangefft = np.fft.fft(adcData, rangefft_size, 0) + dopplerfft = np.fft.fft(rangefft, dopplerfft_size, 1) + anglefft = np.fft.fftshift(np.fft.fft(dopplerfft, anglefft_size, 2),2) + + # self.anglefft = anglefft + return anglefft + +def cart2polar(x, y, in_pixels, limit, range_res): + + #Assuming uniform theta resolution + r = np.sqrt((limit-x)**2 + (y)**2)*(1/range_res) + + theta = np.arctan2((y),(limit-x)) + theta_px = np.sin(theta)*(in_pixels/2) + in_pixels/2 + return r, theta_px + +def polar_to_cart(RATensor, range_res = 150/256, in_pixels = 256, limit = 150, out_pixels = 256): + + """ + convert a polar range angle tensor to cartesian array + Input: + RATensor: NxM + Output: + cart_RATensor: NxM + """ + + X, Y = np.meshgrid(np.linspace(0, limit, out_pixels), + np.linspace(-limit/2, limit/2, out_pixels)) + + R_samp, Theta_samp = cart2polar(X, Y, in_pixels, limit, range_res) + + R_samp = R_samp.astype(int) + Theta_samp = Theta_samp.astype(int) + + R_samp[(R_samp>(in_pixels-1))] = 0 + R_samp[(R_samp<0)] = 0 + Theta_samp[(Theta_samp>(in_pixels-1))] = 0 + Theta_samp[(Theta_samp<0)] = 0 + + + if RATensor.ndim >2: + polar_img = RATensor[...,R_samp,Theta_samp] + # polar_img = torch.reshape(polar_img,(RATensor.shape[0],RATensor.shape[1],out_pixels,out_pixels)) + if torch.is_tensor(RATensor): + polar_img = torch.transpose(polar_img,-1,-2) + else: + polar_img = np.swapaxes(polar_img,-2,-1) + else: + polar_img = RATensor[R_samp,Theta_samp] + polar_img = torch.reshape(polar_img,(out_pixels,out_pixels)) + polar_img = polar_img.T + + return polar_img + + + +def plot_data(data, ax = None): + + # if not ax: + # fig, ax = plt.subplots(1, len(data)) + + for num, im in enumerate(data): + ax[num].imshow(im) + + # plt.show() + + return ax + # plt.imshow(polar_image[0,0]) + # plt.figure() + # plt.imshow(cart_image[0,0]) + +def CFAR_filtered_output(): + ##Cfar layers + guard_cell = 5 + CFAR_cell = 11 + guard_avg = nn.AvgPool2d(guard_cell, stride = 1, padding = 2, divisor_override = 1, count_include_pad=False) + CFAR_avg = nn.AvgPool2d(CFAR_cell, stride = 1, padding = 5, divisor_override=1, count_include_pad=False) + + ##Input + data = sio.loadmat("lid2rad10.mat") + a = data["plot_data"] + x_axis = data["x_axis"] + y_axis = data["y_axis"] + + (size1,size2) = x_axis.shape + + ##CFAR processing + a = torch.tensor(a,dtype=torch.float) + + b = guard_avg(a[None,:,:]) + c = CFAR_avg(a[None,:,:]) + + CA_region = (c-b)/(CFAR_cell**2-guard_cell**2) + thrshold = 10 + + cfar_indices = np.squeeze(a)>np.squeeze(CA_region)*thrshold + cfar_indices[[0,1,2,size2-3,size2-2,size2-1],:] = 0 + cfar_indices[:,[0,1,2,size2-3,size2-2,size2-1]] = 0 + + imag = np.zeros((size1,size2)) + imag[cfar_indices] = 1 + + x_points = x_axis[cfar_indices] + y_points = y_axis[cfar_indices] + z_points = np.zeros_like(y_points) + + pc = np.vstack((x_points,y_points,z_points)) + pc = pc.T + + ##Plotting + pcd=open3d.open3d.geometry.PointCloud() + pcd.points= open3d.open3d.utility.Vector3dVector(pc) + open3d.open3d.visualization.draw_geometries([pcd]) + + fig, ax = plt.subplots(1,5) + ax[0].imshow(a.numpy()) + ax[1].imshow(np.squeeze(b).numpy()) + ax[2].imshow(np.squeeze(c).numpy()) + ax[3].imshow(np.squeeze(CA_region).numpy()) + ax[4].imshow(imag) + plt.show() + +class CA_CFAR(): + """ + Description: + ------------ + Cell Averaging - Constant False Alarm Rate algorithm + Performs an automatic detection on the input range-Doppler matrix with an adaptive thresholding. + The threshold level is determined for each cell in the range-Doppler map with the estimation + of the power level of its surrounding noise. The average power of the noise is estimated on a + rectangular window, that is defined around the CUT (Cell Under Test). In order the mitigate the effect + of the target reflection energy spreading some cells are left out from the calculation in the immediate + vicinity of the CUT. These cells are the guard cells. + The size of the estimation window and guard window can be set with the win_param parameter. + Implementation notes: + --------------------- + Implementation based on https://github.com/petotamas/APRiL + Parameters: + ----------- + :param win_param: Parameters of the noise power estimation window + [Est. window length, Est. window width, Guard window length, Guard window width] + :param threshold: Threshold level above the estimated average noise power + :type win_param: python list with 4 elements + :type threshold: float + Return values: + -------------- + """ + + def __init__(self, win_param, threshold, rd_size): + win_width = win_param[0] + win_height = win_param[1] + guard_width = win_param[2] + guard_height = win_param[3] + + # Create window mask with guard cells + self.mask = np.ones((2 * win_height + 1, 2 * win_width + 1), dtype=bool) + self.mask[win_height - guard_height:win_height + 1 + guard_height, win_width - guard_width:win_width + 1 + guard_width] = 0 + + # Convert threshold value + self.threshold = 10 ** (threshold / 10) + + # Number cells within window around CUT; used for averaging operation. + self.num_valid_cells_in_window = signal.convolve2d(np.ones(rd_size, dtype=float), self.mask, mode='same') + + def __call__(self, rd_matrix): + """ + Description: + ------------ + Performs the automatic detection on the input range-Doppler matrix. + Implementation notes: + --------------------- + Parameters: + ----------- + :param rd_matrix: Range-Doppler map on which the automatic detection should be performed + :type rd_matrix: R x D complex numpy array + Return values: + -------------- + :return hit_matrix: Calculated hit matrix + """ + # Convert range-Doppler map values to power + rd_matrix = np.abs(rd_matrix) ** 2 + + # Perform detection + rd_windowed_sum = signal.convolve2d(rd_matrix, self.mask, mode='same') + rd_avg_noise_power = rd_windowed_sum / self.num_valid_cells_in_window + rd_snr = rd_matrix / rd_avg_noise_power + hit_matrix = rd_snr > self.threshold + + return hit_matrix + + +if __name__ == "__main__": + + sim_adc = sio.loadmat("/home/Kshitiz/semantic_lidar/radar/0099.mat") + sim_adc_data = sim_adc['adc_data'] + sim_adc_data = np.transpose(sim_adc_data, (2,0,1)) + + rangefft_size = 256 + anglefft_size = 256 + dopplerfft_size = 128 + + sim_RDA = convert_adc_to_3d_fft(sim_adc_data ,rangefft_size,dopplerfft_size, anglefft_size, distance_null=False) + + sim_dra = np.transpose(sim_RDA,(1,0,2)) + + limit = 50 + + sim_dra_cart = abs(polar_to_cart(sim_dra,limit=limit,out_pixels=256)) + + fig ,ax = plt.subplots(1,2) + + ax[0].imshow(abs(sim_dra[0,:,:])) + ax[1].imshow(abs(sim_dra_cart[0,:,:])) + + plt.show() \ No newline at end of file diff --git a/scripts/ISOLATE/model_wrapper.py b/scripts/ISOLATE/model_wrapper.py new file mode 100644 index 0000000..2d20bfd --- /dev/null +++ b/scripts/ISOLATE/model_wrapper.py @@ -0,0 +1,118 @@ +import sys +import os +import numpy as np + +# Add the necessary directories to sys.path to ensure internal imports work +current_dir = os.path.dirname(os.path.abspath(__file__)) +package_root = os.path.join(current_dir, 'e2e_agent_sem_lidar2shenron_package') +utils_root = os.path.join(current_dir, 'sim_radar_utils') + +if current_dir not in sys.path: + sys.path.append(current_dir) +if package_root not in sys.path: + sys.path.append(package_root) +if utils_root not in sys.path: + sys.path.append(utils_root) + +# Now import the modules +from e2e_agent_sem_lidar2shenron_package.lidar import run_lidar +from e2e_agent_sem_lidar2shenron_package.ConfigureRadar import radar +from sim_radar_utils.radar_processor import RadarProcessor +from sim_radar_utils.utils_radar import reformat_adc_shenron + +class ShenronRadarModel: + def __init__(self, radar_type='radarbook'): + """ + Initialize the Shenron Radar Model. + + Args: + radar_type (str): Type of radar to simulate (default: 'radarbook'). + """ + print(f"Initializing ShenronRadarModel with type: {radar_type}") + self.radar_type = radar_type + + # Initialize the hardware radar object + self.radar_obj = radar(radar_type) + self.radar_obj.center = np.array([0.0, 0.0]) # center of radar + self.radar_obj.elv = np.array([0.0]) + + # Synchronize global config used by Signal Processor with the Simulated Hardware + self._sync_configs() + + # Initialize the signal processor (FFT, CFAR, etc.) + self.processor = RadarProcessor() + + # Standard simulation config used by the internal physics engine + self.sim_config = { + 'RADAR_TYPE': radar_type, + 'INVERT_ANGLE': 0, + 'RAY_TRACING': False, + 'RADAR_MOVING': False + } + + def _sync_configs(self): + """Important: Sync global variables in sim_radar_utils to match current radar.obj""" + import sim_radar_utils.utils_radar as ur + + # Update Radar Cfg + ur.radarCfg['N'] = self.radar_obj.N_sample + ur.radarCfg['Np'] = self.radar_obj.chirps + ur.radarCfg['NrChn'] = self.radar_obj.nRx + ur.radarCfg['fStrt'] = self.radar_obj.f + ur.radarCfg['fStop'] = self.radar_obj.f + self.radar_obj.B + ur.radarCfg['Tp'] = self.radar_obj.chirp_rep + + # Update FFT Cfg + ur.fftCfg['NFFT'] = self.radar_obj.N_sample + ur.fftCfg['NFFTVel'] = self.radar_obj.chirps + + print(f"Synced global config: N={ur.radarCfg['N']}, Np={ur.radarCfg['Np']}, Ant={ur.radarCfg['NrChn']}") + + def process(self, semantic_lidar_data): + """ + Process semantic LiDAR data to generate a rich radar point cloud. + + Args: + semantic_lidar_data (np.ndarray): Array of shape [N, 7] + format: [x, y, z, intensity, cos_inc_angle, object_idx, semantic_tag] + + Returns: + np.ndarray: Rich radar point cloud [M, 5] + format: [x, y, z, velocity, magnitude] + """ + if semantic_lidar_data is None or len(semantic_lidar_data) == 0: + return np.empty((0, 5)) + + try: + # 1. Physics-based Signal Generation (FMCW Chirps) + # This generates the raw ADC samples [Np, N, Ant] + adc_data = run_lidar(self.sim_config, semantic_lidar_data) + + # 2. Reformat to match Signal Processor expectations + # Internal logic often needs specific axis ordering + adc_data = reformat_adc_shenron(adc_data) + + # 3. Fast Fourier Transform (FFT) Pipeline + # Range FFT converts time data to range profiles + range_profile = self.processor.cal_range_fft(adc_data) + + # Doppler FFT converts range profiles over time to velocity info + doppler_profile = self.processor.cal_doppler_fft(range_profile) + + # 4. Target Detection and Rich Parameter Extraction + # CFAR detection + Angle of Arrival (AoA) estimation + # returns: rangeAoA, pointcloud ([x, y, z, vel, mag]) + _, rich_pcd = self.processor.convert_to_pcd(doppler_profile) + + return rich_pcd + + except Exception as e: + print(f"Error during Shenron processing: {e}") + import traceback + traceback.print_exc() + return np.empty((0, 5)) + +if __name__ == "__main__": + # Internal test/demo + model = ShenronRadarModel() + print("Model initialized successfully.") diff --git a/scripts/ISOLATE/sim_radar_utils/cfar_detector.py b/scripts/ISOLATE/sim_radar_utils/cfar_detector.py new file mode 100644 index 0000000..7746422 --- /dev/null +++ b/scripts/ISOLATE/sim_radar_utils/cfar_detector.py @@ -0,0 +1,70 @@ +import numpy as np +from scipy import signal + +class CA_CFAR(): + """ + Description: + ------------ + Cell Averaging - Constant False Alarm Rate algorithm + Performs an automatic detection on the input range-Doppler matrix with an adaptive thresholding. + The threshold level is determined for each cell in the range-Doppler map with the estimation + of the power level of its surrounding noise. The average power of the noise is estimated on a + rectangular window, that is defined around the CUT (Cell Under Test). In order the mitigate the effect + of the target reflection energy spreading some cells are left out from the calculation in the immediate + vicinity of the CUT. These cells are the guard cells. + The size of the estimation window and guard window can be set with the win_param parameter. + Implementation notes: + --------------------- + Implementation based on https://github.com/petotamas/APRiL + Parameters: + ----------- + :param win_param: Parameters of the noise power estimation window + [Est. window length, Est. window width, Guard window length, Guard window width] + :param threshold: Threshold level above the estimated average noise power + :type win_param: python list with 4 elements + :type threshold: float + Return values: + -------------- + """ + + def __init__(self, win_param, threshold, rd_size): + win_width = win_param[0] + win_height = win_param[1] + guard_width = win_param[2] + guard_height = win_param[3] + + # Create window mask with guard cells + self.mask = np.ones((2 * win_height + 1, 2 * win_width + 1), dtype=bool) + self.mask[win_height - guard_height:win_height + 1 + guard_height, win_width - guard_width:win_width + 1 + guard_width] = 0 + + # Convert threshold value + self.threshold = 10 ** (threshold / 10) + + # Number cells within window around CUT; used for averaging operation. + self.num_valid_cells_in_window = signal.convolve2d(np.ones(rd_size, dtype=float), self.mask, mode='same') + + def __call__(self, rd_matrix): + """ + Description: + ------------ + Performs the automatic detection on the input range-Doppler matrix. + Implementation notes: + --------------------- + Parameters: + ----------- + :param rd_matrix: Range-Doppler map on which the automatic detection should be performed + :type rd_matrix: R x D complex numpy array + Return values: + -------------- + :return hit_matrix: Calculated hit matrix + """ + # Convert range-Doppler map values to power + rd_matrix = np.abs(rd_matrix) ** 2 + + # Perform detection + rd_windowed_sum = signal.convolve2d(rd_matrix, self.mask, mode='same') + rd_avg_noise_power = rd_windowed_sum / self.num_valid_cells_in_window + rd_snr = rd_matrix / rd_avg_noise_power + hit_matrix = rd_snr > self.threshold + + return hit_matrix \ No newline at end of file diff --git a/scripts/ISOLATE/sim_radar_utils/config.yaml b/scripts/ISOLATE/sim_radar_utils/config.yaml new file mode 100644 index 0000000..564c8ac --- /dev/null +++ b/scripts/ISOLATE/sim_radar_utils/config.yaml @@ -0,0 +1,101 @@ +radar: + fStrt: 24.0e9 # start frequency + fStop: 24.25e9 # stop frequency + TRampUp: 100e-6 # ramp-up time of chirp + TInt: 100e-3 # inter-frame interval + Tp: 1000e-6 # inter-chirp time + N: 256 # samples per chirp + IniTim: 100e-3 # initial delay before collection of samples + IniEve: 0 # start automatically after IniTim +# TxSeq: [1, 2] + TxSeq: [1] # antenna activation sequence + Np: 3 #128 # number of chirps in one frame + AntIdx: [0, 1, 2, 3, 4, 5, 6, 7, 9, 10, 11, 12, 13, 14, 15] + # AntIdx: [0, 1, 2, 3, 4, 5, 6, 7] + NrChn: 86 #16 + + +FFT: + NFFT: 256 #512 + NFFTVel: 128 + NFFTAnt: 256 #128 + RMin: 0 + RMax: 1000 + c0: 299792458 + + +Cluster: + xlim: [-20, 20] + ylim: [0, 40] + zlim: [0, 2] + + +ROS: + Topic: + radarCfg: '/radarbook/config' + radarRaw: '/radarbook/data' + radarPCD: '/radarbook/pcd' + cameraRGB: '/camera/color/image_raw' + cameraDepth: '/camera/depth/image_rect_raw' + lidarPCD: '/os1_cloud_node/points' + SavePath: + radarCfg: '/radar_cfg.json' + radarRaw: '/radar_data/' + radarPCD: '/radar_pcd/' + cameraRGB: '/rgb_image/' + # cameraRGB: '/camera_original/' + cameraDepth: '/depth_image/' + # lidarPCD: '/lidar_pcd/' + lidarPCD: '/raw_lidar/' + timeStamp: '/time_stamp.npy' + +CFAR: + win_param: [9, 9, 3, 3] + threshold: 20 + + +Visualize: + rangeAoA: + title: "Range-Angle Plot" + xLabel: "Range" + xUnit: "m" + yLabel: "Angle" + yUnit: "Degree" + winSize: [500, 400] + pos: [50, 50] + rangeDoppler: + title: "Range-Velocity Plot" + xLabel: "Range" + xUnit: "m" + yLabel: "Velocity" + yUnit: "m/s" + winSize: [500, 400] + pos: [600, 50] + radarPCD: + title: "Radar Point Cloud" + xLabel: "Azimuth Position" + xUnit: "m" + yLabel: "Depth Position" + yUnit: "m" + winSize: [500, 400] + pos: [1200, 50] + penWidth: 5 + penColor: 'r' + symbol: 'o' + dotSize: 1 + cameraRGB: + title: "RGB Image" + winSize: [640, 360] + pos: [50, 500] + cameraDepth: + title: "Depth Image" + winSize: [640, 360] + pos: [600, 500] + lidarPCD: + title: "Lidar Point Cloud" + xRange: [-30, 30] + yRange: [-30, 30] + winSize: [500, 400] + pos: [1200, 500] + dotSize: 1 + diff --git a/scripts/ISOLATE/sim_radar_utils/convert2D_img.py b/scripts/ISOLATE/sim_radar_utils/convert2D_img.py new file mode 100644 index 0000000..44ac9a6 --- /dev/null +++ b/scripts/ISOLATE/sim_radar_utils/convert2D_img.py @@ -0,0 +1,116 @@ +import numpy as np +import os +import pickle as pkl +from PIL import Image +import json +import yaml +# import ffmpeg +import matplotlib.pyplot as plt +# import cv2 +import sys +import glob + +# sys.path.append('/radar-imaging-dataset/mmfn_project/mmfn_scripts/team_code/mmfn_utils/sim_radar_utils/') +# sys.path.append('/radar-imaging-dataset/mmfn_project/mmfn_scripts/team_code/e2e_agent_sem_lidar2shenron_package/') + +from e2e_agent_sem_lidar2shenron_package.lidar import run_lidar + +from sim_radar_utils.radar_processor import RadarProcessor +from sim_radar_utils.utils_radar import * +from sim_radar_utils.transform_utils import * + +def convert2D_img_func(sim_radar, limit = 75): + ''' + converts the 3D radar raw data to 2d range-angle image of dimension 256X256 + ''' + radarProcessor = RadarProcessor() + # radar = sim_radar[0] + radar = sim_radar + + chirpLevelData = reformat_adc_shenron(radar) + rangeProfile = radarProcessor.cal_range_fft(chirpLevelData) + # dopplerProfile = radarProcessor.cal_doppler_fft(rangeProfile) + aoaProfile = radarProcessor.cal_angle_fft(rangeProfile) + range_angle = get_range_angle(aoaProfile) + cart_cord = polar_to_cart(range_angle, limit = limit) + # print(cart_cord_man.shape) + # return [cart_cord] + return cart_cord + +def convert_sem_lidar_2D_img_func(sim_radar, invert_angle, limit = 75): + ''' + converts the semantic_lidar data to the range angle 2D image of dimension 256X256 + ''' + + # convert semantic lidar to raw 3d radar data + + # with open('/radar-imaging-dataset/mmfn_project/mmfn_scripts/team_code/e2e_agent_sem_lidar2shenron_package/simulator_configs.yaml', 'r') as f: + # sim_config = yaml.safe_load(f) + + # with open(glob.glob('../carla_garage_radar/team_code/e2e_agent_sem_lidar2shenron_package/simulator_configs.yaml'), 'r') as f: + # sim_config = yaml.safe_load(f) + + with open('/radar-imaging-dataset/carla_garage_radar/team_code/e2e_agent_sem_lidar2shenron_package/simulator_configs.yaml', 'r') as f: + sim_config = yaml.safe_load(f) + + sim_config['INVERT_ANGLE'] = invert_angle + + radar = run_lidar(sim_config, sim_radar) + + radarProcessor = RadarProcessor() + # radar = sim_radar[0] + # radar = sim_radar + + chirpLevelData = reformat_adc_shenron(radar) + rangeProfile = radarProcessor.cal_range_fft(chirpLevelData) + # dopplerProfile = radarProcessor.cal_doppler_fft(rangeProfile) + aoaProfile = radarProcessor.cal_angle_fft(rangeProfile) + range_angle = get_range_angle(aoaProfile) + cart_cord = polar_to_cart(range_angle, limit = limit) + # print(cart_cord_man.shape) + # return [cart_cord] + return cart_cord + +def main(): + radarProcessor = RadarProcessor() + + # with open('/mnt/intA-ssdr1-4tb/satyam53/mmfn/data1/pro_train_fnss_0702/0.pkl', 'rb') as f: + # pkl_data = pkl.load(f) + + # print(type(pkl_data)) + # print(pkl_data.keys()) + + # sim_radar = pkl_data['sim_radar'] + # # sim_radar = pkl_data['vectormaps'] + + # print(type(sim_radar[0])) + # print(len(sim_radar)) + # print(sim_radar[0]) + # radar_man = sim_radar[0] + + + # # radar_man = np.load(f'{path_sim_man}/{files_sim_manual[i]}') + # chirpLevelData = reformat_adc_shenron(radar_man) + # rangeProfile = radarProcessor.cal_range_fft(chirpLevelData) + # # dopplerProfile = radarProcessor.cal_doppler_fft(rangeProfile) + # aoaProfile = radarProcessor.cal_angle_fft(rangeProfile) + # range_angle = get_range_angle(aoaProfile) + # cart_cord_man = polar_to_cart(range_angle, limit = 50) + # print(cart_cord_man.shape) + + # fig, (ax1, ax2, ax3) = plt.subplots(1, 3, figsize=(15,5)) + # # ax1.imshow(cart_cord, aspect='auto') + # # ax1.set_title('radar data simulated using shenron') + + # ax2.imshow(cart_cord_man, aspect='auto') + # ax2.set_title('radar data simulated manually') + # # plt.show() + # # ax3.scatter(lidar[:, 0], lidar[:, 1], s = 0.1, c = lidar[:, 5]) + # # ax3.set_title('lidar data') + # plt.tight_layout() + # plt.show() + # # plt.savefig(f'{save_path}/{i}.png') + # plt.close() + +if __name__=="__main__": + main() \ No newline at end of file diff --git a/scripts/ISOLATE/sim_radar_utils/radar_processor.py b/scripts/ISOLATE/sim_radar_utils/radar_processor.py new file mode 100644 index 0000000..19ac25b --- /dev/null +++ b/scripts/ISOLATE/sim_radar_utils/radar_processor.py @@ -0,0 +1,96 @@ +import numpy as np +import yaml +import sys +import glob + +# sys.path.append(glob.glob('../carla_garage_radar/team_code/sim_radar_utils/')) +sys.path.append('radar-imaging-dataset/carla_garage_radar/team_code/sim_radar_utils/') +# sys.path.append('radar-imaging-dataset/carla_garage_radar/team_code/') + + +# sys.path.append("/radar-imaging-dataset/mmfn_project/mmfn_scripts/team_code/sim_radar_utils/") + +from scipy import signal +from scipy.fft import fft, fftshift +from sim_radar_utils.cfar_detector import CA_CFAR +from sim_radar_utils.utils_radar import * + + +cfarCfg = config['CFAR'] + + +class RadarProcessor: + def __init__(self): + # radar data will be shaped as (# of chirp, # of sample, # of antenna) + # self.rangeWin = np.tile(signal.windows.hann(radarCfg['N']), (radarCfg['Np'], len(radarCfg['AntIdx']), 1)) + self.rangeWin = np.tile(signal.windows.hann(radarCfg['N']), (radarCfg['Np'], radarCfg['NrChn'], 1)) + self.rangeWin = np.transpose(self.rangeWin, (2, 0, 1)) + + # self.velWin = np.tile(signal.windows.hann(radarCfg['Np']), (radarCfg['N'], len(radarCfg['AntIdx']), 1)) + self.velWin = np.tile(signal.windows.hann(radarCfg['Np']), (radarCfg['N'], radarCfg['NrChn'], 1)) + self.velWin = np.transpose(self.velWin, (0, 2, 1)) + + rangeRes = fftCfg['c0'] / (2*(radarCfg['fStop'] - radarCfg['fStrt'])) + self.rangeAxis = np.arange(0, fftCfg['NFFT'])*radarCfg['N']/fftCfg['NFFT']*rangeRes + self.RMinIdx = np.argmin(np.abs(self.rangeAxis - fftCfg['RMin'])) + self.RMaxIdx = np.argmin(np.abs(self.rangeAxis - fftCfg['RMax'])) + self.rangeAxis = self.rangeAxis[self.RMinIdx:self.RMaxIdx+1] + + self.angleAxis = np.arcsin(2 * np.arange(-fftCfg['NFFTAnt']/2, fftCfg['NFFTAnt']/2) / fftCfg['NFFTAnt']) + + fc = (radarCfg['fStop'] + radarCfg['fStrt'])/2 + self.velAxis = np.arange(-fftCfg['NFFTVel']//2, fftCfg['NFFTVel']//2)/fftCfg['NFFTVel']*(1/radarCfg['Tp'])*fftCfg['c0']/(2*fc) + + self.cfar = CA_CFAR(win_param=cfarCfg['win_param'], + threshold=cfarCfg['threshold'], + rd_size=(self.RMaxIdx - self.RMinIdx + 1, fftCfg['NFFTVel'])) + + def cal_range_fft(self, data): + '''apply range window and doppler window and apply fft on each sample to get range profile''' + return fft(data * self.rangeWin * self.velWin, fftCfg['NFFT'], 0) + + def cal_doppler_fft(self, rangeProfile): + '''apply fft on each chirp to get doppler profile''' + return fftshift(fft(rangeProfile[self.RMinIdx:self.RMaxIdx+1, :], fftCfg['NFFTVel'], 1), 1) + + def cal_angle_fft(self, rangeProfile): + '''# apply fft on each antenna to get angle profile''' + return fftshift(fft(rangeProfile[self.RMinIdx:self.RMaxIdx+1, :], fftCfg['NFFTAnt'], 2), 2) + + def convert_to_pcd(self, dopplerProfile): + avgDopplerProfile = np.squeeze(np.mean(dopplerProfile, 2)) + + # detect useful peaks using CFAR + detections = self.cfar(np.square(np.abs(avgDopplerProfile))) + + # identify range bin and velocity bin for each detected point + rowSel, colSel = np.nonzero(detections) + + # pointSel = np.zeros(shape=(len(rowSel), len(radarCfg['AntIdx'])), dtype=complex) + pointSel = np.zeros(shape=(len(rowSel), radarCfg['NrChn']), dtype=complex) + for i, (row, col) in enumerate(zip(rowSel, colSel)): + pointSel[i] = dopplerProfile[row, col, :] + + # calculate range and anlge value + rangeVals = self.rangeAxis[rowSel] + aoaProfile = fftshift(fft(pointSel, fftCfg['NFFTAnt'], 1), 1) + angleIdx = np.argmax(np.abs(aoaProfile), axis=1) + angleVals = self.angleAxis[angleIdx] + + # Extract velocity from velocity axis + velVals = self.velAxis[colSel] + + # Extract peak magnitude + magVals = np.abs(avgDopplerProfile[rowSel, colSel]) + + rangeAoA = np.transpose(np.stack([rangeVals, angleVals])) + + # convert Range-AoA to pointcloud + # Format: [x, y, z, velocity, magnitude] + x = rangeVals * np.cos(angleVals) + y = rangeVals * np.sin(angleVals) + z = np.zeros_like(x) # 2D radar assumption, z=0 + + pointcloud = np.stack([x, y, z, velVals, magVals], axis=1) + + return rangeAoA, pointcloud diff --git a/scripts/ISOLATE/sim_radar_utils/transform_utils.py b/scripts/ISOLATE/sim_radar_utils/transform_utils.py new file mode 100644 index 0000000..b7d748c --- /dev/null +++ b/scripts/ISOLATE/sim_radar_utils/transform_utils.py @@ -0,0 +1,91 @@ +import numpy as np +import torch + +def get_range_angle(raw_radar_data): + range_angle = np.abs(raw_radar_data).sum(axis=1) + return range_angle + +def cart2polar(x, y, in_pixels, range_res): + # Dividing by range_res to get the range in pixels + r = np.sqrt(x ** 2 + y ** 2) * (1 / range_res) + + #Assuming uniform theta resolution + theta = np.arctan2(y, x) + theta_px = np.sin(theta) * (in_pixels / 2) + in_pixels / 2 + return r, theta_px + +def polar_to_cart(RATensor, range_res = 150 / 256, in_pixels = 256, limit = 150, out_pixels = 256): + + """ + convert a polar range angle tensor to cartesian array + Input: + RATensor: NxM + Output: + cart_RATensor: NxM + """ + + X, Y = np.meshgrid(np.linspace(-limit/2, limit/2, out_pixels), np.linspace(-limit/2, limit/2, out_pixels)) + # Rotating the axes to make the plane point upwards + X = np.rot90(X) + Y = np.rot90(Y) + + R_samp, Theta_samp = cart2polar(X, Y, in_pixels, range_res) + + R_samp = R_samp.astype(int) + Theta_samp = Theta_samp.astype(int) + + R_samp[(R_samp > (in_pixels - 1))] = in_pixels - 1 + R_samp[(R_samp < 0)] = 0 + Theta_samp[(Theta_samp > (in_pixels - 1))] = in_pixels - 1 + Theta_samp[(Theta_samp < 0)] = 0 + + if RATensor.ndim >2: + polar_img = RATensor[...,R_samp,Theta_samp] + else: + polar_img = RATensor[R_samp, Theta_samp] + + # Applying mask to extract the front-side view only + if out_pixels % 2 == 0: + slice_idx = int(out_pixels / 2) + else: + slice_idx = int(out_pixels / 2) + 1 + + polar_img[slice_idx:, :] = 0 + + return polar_img + +# def polar_to_cart(RATensor, range_res = 150/256, in_pixels = 256, limit = 150, out_pixels = 256): + +# """ +# convert a polar range angle tensor to cartesian array +# Input: +# RATensor: NxM +# Output: +# cart_RATensor: NxM +# """ + +# X, Y = np.meshgrid(np.linspace(0, limit, out_pixels), np.linspace(-limit/2, limit/2, out_pixels)) + +# R_samp, Theta_samp = cart2polar(X, Y, in_pixels, range_res) + +# R_samp = R_samp.astype(int) +# Theta_samp = Theta_samp.astype(int) + +# R_samp[(R_samp>(in_pixels-1))] = 0 +# R_samp[(R_samp<0)] = 0 +# Theta_samp[(Theta_samp>(in_pixels-1))] = 0 +# Theta_samp[(Theta_samp<0)] = 0 + +# if RATensor.ndim >2: +# polar_img = RATensor[...,R_samp,Theta_samp] +# # polar_img = torch.reshape(polar_img,(RATensor.shape[0],RATensor.shape[1],out_pixels,out_pixels)) +# if torch.is_tensor(RATensor): +# polar_img = torch.transpose(polar_img,-1,-2) +# else: +# polar_img = np.swapaxes(polar_img,-2,-1) +# else: +# polar_img = RATensor[R_samp,Theta_samp] +# # polar_img = torch.reshape(polar_img,(out_pixels,out_pixels)) +# polar_img = polar_img.T + +# return polar_img \ No newline at end of file diff --git a/scripts/ISOLATE/sim_radar_utils/utils_radar.py b/scripts/ISOLATE/sim_radar_utils/utils_radar.py new file mode 100644 index 0000000..9f37a0b --- /dev/null +++ b/scripts/ISOLATE/sim_radar_utils/utils_radar.py @@ -0,0 +1,112 @@ +import sys, os +import yaml +import numpy as np +# import open3d +from pyntcloud import PyntCloud +from PIL import Image +import matplotlib.pyplot as plt +import json + + +with open(os.path.join(os.path.abspath(os.path.dirname(__file__)), 'config.yaml'), 'r') as f: + config = yaml.safe_load(f) + radarCfg = config['radar'] + savePath = config['ROS']['SavePath'] + fftCfg = config['FFT'] + clstCfg = config['Cluster'] + radarCfg['fStrt'] = float(radarCfg['fStrt']) + radarCfg['fStop'] = float(radarCfg['fStop']) + radarCfg['TRampUp'] = float(radarCfg['TRampUp']) + radarCfg['TInt'] = float(radarCfg['TInt']) + radarCfg['Tp'] = float(radarCfg['Tp']) + radarCfg['IniTim'] = float(radarCfg['IniTim']) + + +def config_save_path(dirc): + for node in savePath.keys(): + savePath[node] = dirc + savePath[node] + + +def create_folder(activeNode): + for node in activeNode: + if node != 'timeStamp' and node != 'radarCfg': + os.makedirs(savePath[node], exist_ok=True) + + +def read_time_stamp(): + return np.load(savePath['timeStamp']) + + +def load_radar_cfg(): + with open(savePath['radarCfg'], 'r') as f: + newCfg = json.load(f) + for key in radarCfg.keys(): + radarCfg[key] = newCfg[key] + +#reformat adc shenron +def reformat_adc_shenron(data): + data = np.swapaxes(data, 1, 2) + # print(data.shape) + chirpLevelData = data + # return np.transpose(chirpLevelData[:, :, radarCfg['AntIdx']], (1, 0, 2)) + return np.transpose(chirpLevelData[:, :, :], (1, 0, 2)) + + +def reformat_adc(data): + data = np.reshape(data, (radarCfg['Np'], len(radarCfg['TxSeq']), radarCfg['N'], radarCfg['NrChn'])) + chirpLevelData = data[:, 0, :, :] + for j in range(1, len(radarCfg['TxSeq'])): + chirpLevelData = np.concatenate((chirpLevelData, data[:, j, :, :]), axis=-1) + return np.transpose(chirpLevelData[:, :, radarCfg['AntIdx']], (1, 0, 2)) + + +def read_radar_data(frame): + radarData = np.load(savePath['radarRaw'] + f'{frame:04}.npy') + chirpLevelData = reformat_adc(radarData) + return chirpLevelData + +#read radar data shenron +def read_radar_data_shenron(frame): + radarData = np.load(savePath['radarRaw'] + f'{frame:04}.npy') + chirpLevelData = reformat_adc_shenron(radarData) + return chirpLevelData + + +def read_radar_pcd(frame): + points = np.load(savePath['radarPCD'] + f'{frame:04}.npy') + return points + + +def read_camera_img(frame): + rgbImg = np.asarray(Image.open(savePath['cameraRGB'] + f'{frame:04}.png')) + depthImg = np.asarray(Image.open(savePath['cameraDepth'] + f'{frame:04}.png')) + return rgbImg, depthImg + + +def read_lidar_pcd(frame): + pcd = PyntCloud.from_file(savePath['lidarPCD'] + f'{frame:04}.pts') + return pcd.xyz + + +def plot_range_doppler(vis, dopplerProfile, rangeAxis, velAxis): + dopplerProfiledB = 20*np.log10(np.abs(dopplerProfile[:, :, 0])) + dopplerProfileMax = np.max(dopplerProfiledB) + dopplerProfileNorm = dopplerProfiledB - dopplerProfileMax + dopplerProfileNorm[dopplerProfileNorm < -30] = -30 + vis.plot_depth_fig( + data=dopplerProfileNorm, + pos=[velAxis[0], fftCfg['RMin']], + scale=[(velAxis[-1] - velAxis[0])/fftCfg['NFFTVel'], (fftCfg['RMax'] - fftCfg['RMin'])/len(rangeAxis)] + ) + + +def plot_range_aoa(vis, aoaProfile, rangeAxis): + aoaProfiledB = 20*np.log10(np.abs(aoaProfile[:, 1, :])) + aoaProfileMax = np.max(aoaProfiledB) + aoaProfileNorm = aoaProfiledB - aoaProfileMax + aoaProfileNorm[aoaProfileNorm < -30] = -30 + vis.plot_depth_fig( + data=aoaProfileNorm, + pos=[-1, fftCfg['RMin']], + scale=[2.0/fftCfg['NFFTAnt']/np.pi*180, (fftCfg['RMax'] - fftCfg['RMin'])/len(rangeAxis)] + ) \ No newline at end of file diff --git a/scripts/data_to_mcap.py b/scripts/data_to_mcap.py index 4b736fd..db850e7 100644 --- a/scripts/data_to_mcap.py +++ b/scripts/data_to_mcap.py @@ -97,6 +97,7 @@ def convert_folder(folder_path): lidar_channel_id = writer.register_channel(topic="/lidar", message_encoding="json", schema_id=lidar_schema_id) pose_channel_id = writer.register_channel(topic="/ego_pose", message_encoding="json", schema_id=pose_schema_id) radar_channel_id = writer.register_channel(topic="/radar", message_encoding="json", schema_id=lidar_schema_id) + shenron_channel_id = writer.register_channel(topic="/radar/shenron", message_encoding="json", schema_id=lidar_schema_id) frame_count = 0 for frame in load_frames(folder_path): @@ -189,6 +190,34 @@ def convert_folder(folder_path): } writer.add_message(radar_channel_id, log_time=ts_ns, data=json.dumps(radar_msg).encode(), publish_time=ts_ns) + # SHENRON RADAR + shenron_file = f"frame_{self.frame_id:06d}.npy" + shenron_path = os.path.join(folder_path, "shenron_radar", shenron_file) + if os.path.exists(shenron_path): + s_data = np.load(shenron_path) + if s_data.size > 0: + # s_data = [x, y, z, velocity, magnitude] + # ISOLATE coords: X is fwd, Y is right. + # ROS: X is fwd, Y is left. + ros_shenron = s_data.copy().astype(np.float32) + ros_shenron[:, 1] = -ros_shenron[:, 1] # Negate Y for ROS + + shenron_msg = { + "timestamp": {"sec": ts_sec, "nsec": ts_nsec}, + "frame_id": "ego_vehicle", + "pose": identity_pose, + "point_stride": 20, # 5 floats * 4 bytes + "fields": [ + {"name":"x","offset":0,"type":7}, + {"name":"y","offset":4,"type":7}, + {"name":"z","offset":8,"type":7}, + {"name":"velocity","offset":12,"type":7}, + {"name":"magnitude","offset":16,"type":7} + ], + "data": base64.b64encode(ros_shenron.tobytes()).decode("ascii") + } + writer.add_message(shenron_channel_id, log_time=ts_ns, data=json.dumps(shenron_msg).encode(), publish_time=ts_ns) + frame_count += 1 if frame_count % 50 == 0: print(f" Processed {frame_count} frames...", flush=True) diff --git a/scripts/generate_shenron.py b/scripts/generate_shenron.py new file mode 100644 index 0000000..7b6174b --- /dev/null +++ b/scripts/generate_shenron.py @@ -0,0 +1,95 @@ +import os +import sys +import numpy as np +import tqdm +from pathlib import Path + +# Add project root and ISOLATE paths +project_root = Path(__file__).parent.parent +sys.path.append(str(project_root)) +sys.path.append(str(project_root / 'scripts' / 'ISOLATE')) + +# Import the model wrapper +try: + from scripts.ISOLATE.model_wrapper import ShenronRadarModel +except ImportError as e: + print(f"Error: Failed to import ShenronRadarModel. Ensure scripts/ISOLATE/model_wrapper.py exists. ({e})") + sys.exit(1) + +def process_session(session_path): + print(f"\n>>> Processing session: {session_path.name}") + + lidar_dir = session_path / "lidar" + if not lidar_dir.exists(): + print(f" [SKIP] No 'lidar' folder found.") + return + + # Find all .npy files in lidar/ + lidar_files = sorted(list(lidar_dir.glob("*.npy"))) + if not lidar_files: + print(f" [SKIP] No .npy files in 'lidar' folder.") + return + + output_dir = session_path / "shenron_radar" + output_dir.mkdir(exist_ok=True) + + # Initialize the model once per session + print(f" Initializing ShenronRadarModel...") + model = ShenronRadarModel(radar_type='radarbook') + + print(f" Generating Shenron Radar data for {len(lidar_files)} frames...") + + for lidar_file in tqdm.tqdm(lidar_files, desc=" Simulating Radar", unit="frame"): + try: + # 1. Load Semantic LiDAR data + # Expected raw: [x, y, z, cos, obj, tag] (6 cols) + # Expected Shenron input: [x, y, z, intensity, cos, obj, tag] (7 cols) + data = np.load(lidar_file) + + if data.shape[1] == 6: + # Pad with a dummy intensity column at index 3 + # This aligns 'tag' to index 6 as expected by our lidar.py mapping + padded_data = np.zeros((data.shape[0], 7)) + padded_data[:, 0:3] = data[:, 0:3] # x, y, z + padded_data[:, 4:7] = data[:, 3:6] # cos, obj, tag + data = padded_data + + # 2. Process through the physics-based model + # returns rich PCD: [M, 5] (x, y, z, velocity, magnitude) + rich_pcd = model.process(data) + + # 3. Save to disk + output_file = output_dir / lidar_file.name + np.save(output_file, rich_pcd) + + except Exception as e: + print(f"\n [ERROR] Failed to process {lidar_file.name}: {e}") + +def main(): + data_root = project_root / "data" + if not data_root.exists(): + print(f"Error: {data_root} not found.") + return + + # Get all session folders + sessions = sorted([d for d in data_root.iterdir() if d.is_dir()]) + + if not sessions: + print("No simulation sessions found in data/.") + return + + print(f"Found {len(sessions)} sessions.") + + for session in sessions: + # Check if the session has frames.jsonl to confirm it's a valid data folder + if (session / "frames.jsonl").exists(): + process_session(session) + else: + print(f"Skipping {session.name} (no frames.jsonl found).") + + print("\n" + "="*50) + print("SHENRON BATCH PROCESSING COMPLETE!") + print("="*50) + +if __name__ == "__main__": + main() diff --git a/scripts/test_shenron_model.py b/scripts/test_shenron_model.py new file mode 100644 index 0000000..728030d --- /dev/null +++ b/scripts/test_shenron_model.py @@ -0,0 +1,98 @@ +import sys +import os +import numpy as np +import time + +# Add project root and ISOLATE paths to sys.path +project_root = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) +sys.path.append(project_root) + +# Import the model wrapper +try: + from scripts.ISOLATE.model_wrapper import ShenronRadarModel + print(">>> Successfully imported ShenronRadarModel") +except ImportError as e: + print(f">>> Failed to import ShenronRadarModel: {e}") + sys.exit(1) + +def run_smoke_test(): + print("\n" + "="*50) + print("SHENRON MODEL SMOKE TEST") + print("="*50) + + success = True + + try: + # 1. Initialize the Model + print("\n[Step 1] Initializing ShenronRadarModel...") + start_time = time.time() + model = ShenronRadarModel(radar_type='radarbook') + print(f"Done in {time.time() - start_time:.2f}s") + + # 2. Prepare Synthetic Semantic LiDAR Data + # Format: [x, y, z, intensity, cos_inc_angle, object_idx, semantic_tag] + print("\n[Step 2] Preparing synthetic semantic LiDAR data...") + # Create a "vehicle" (tag 10) 15 meters ahead, slightly to the left + num_points = 100 + data = np.zeros((num_points, 7)) + + # Geometry + data[:, 0] = np.random.uniform(14.5, 15.5, num_points) # x (forward) + data[:, 1] = np.random.uniform(-2.0, -1.0, num_points) # y (left) + data[:, 2] = np.random.uniform(0.5, 1.5, num_points) # z (up) + + # Physical properties + data[:, 3] = 0.5 # intensity + data[:, 4] = 0.9 # cos_inc_angle + data[:, 5] = 123 # object_idx + data[:, 6] = 10 # semantic_tag (Vehicle) + + print(f"Created {num_points} points representing a vehicle at 15m.") + + # 3. Process data through the model + print("\n[Step 3] Running Shenron Radar Simulation (FMCW + DSP)...") + start_time = time.time() + rich_pcd = model.process(data) + process_time = time.time() - start_time + print(f"Done in {process_time:.2f}s") + + # 4. Validate output + print("\n[Step 4] Validating output...") + print(f"Output shape: {rich_pcd.shape}") + + if rich_pcd.shape[0] > 0: + print(f"Detected {rich_pcd.shape[0]} radar targets.") + print("Format: [x, y, z, velocity, magnitude]") + # Check for expected coordinates (roughly 15m ahead) + avg_x = np.mean(rich_pcd[:, 0]) + avg_y = np.mean(rich_pcd[:, 1]) + print(f"Average Detection Position: x={avg_x:.2f}, y={avg_y:.2f}") + + if 10 < avg_x < 20: + print("SUCCESS: Detections are in the expected range.") + else: + print("WARNING: Detections are outside expected range.") + + print("\nSample Detections:") + print(rich_pcd[:5]) + else: + print("ERROR: No detections found from valid input data.") + success = False + + except Exception as e: + print(f"\n!!! CRITICAL ERROR during smoke test: {e}") + import traceback + traceback.print_exc() + success = False + + print("\n" + "="*50) + if success: + print("RESULT: SMOKE TEST PASSED") + else: + print("RESULT: SMOKE TEST FAILED") + print("="*50) + + return success + +if __name__ == "__main__": + run_smoke_test() diff --git a/src/recorder.py b/src/recorder.py index d4ea661..e2df95f 100644 --- a/src/recorder.py +++ b/src/recorder.py @@ -79,9 +79,14 @@ class Recorder: radar_path = os.path.join(self.radar_path, radar_file) np.save(radar_path, radar_np) - # -------- LIDAR -------- + # -------- LIDAR (Semantic) -------- + # Dynamic reshape to handle semantic columns: [x, y, z, cos, obj, tag] lidar_data = np.frombuffer(lidar.raw_data, dtype=np.float32) - lidar_data = np.reshape(lidar_data, (-1, 4)) # x, y, z, intensity + total_points = len(lidar) + if total_points > 0: + lidar_data = np.reshape(lidar_data, (total_points, -1)) + else: + lidar_data = np.empty((0, 6)) # Default to 6 columns for semantic lidar_file = f"frame_{self.frame_id:06d}.npy" lidar_path = os.path.join(self.lidar_path, lidar_file) diff --git a/src/sensors.py b/src/sensors.py index f0d03e1..55b03cc 100644 --- a/src/sensors.py +++ b/src/sensors.py @@ -86,7 +86,7 @@ class SensorManager: # ---------------- LIDAR ---------------- def _spawn_lidar(self): - lidar_bp = self.bp_lib.find("sensor.lidar.ray_cast") + lidar_bp = self.bp_lib.find("sensor.lidar.ray_cast_semantic") lidar_bp.set_attribute("range", str(config.LIDAR_RANGE)) lidar_bp.set_attribute("channels", str(config.LIDAR_CHANNELS))