Skip to content

Commit

Permalink
Generate new plots
Browse files Browse the repository at this point in the history
  • Loading branch information
@duembgen
duembgen committed Jun 16, 2022
1 parent 46e9f33 commit 9d9dfcc
Show file tree
Hide file tree
Showing 11 changed files with 672 additions and 664 deletions.
156 changes: 107 additions & 49 deletions python/Beamforming.ipynb
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,13 +47,17 @@
"metadata": {},
"outputs": [],
"source": [
"from crazyflie_description_py.parameters import N_BUFFER, FS\n",
"from utils.simulation import WIDEBAND_FILE, create_wideband_signal\n",
"from utils.constants import PLATFORM\n",
"if PLATFORM == \"crazyflie\":\n",
" from crazyflie_description_py.parameters import N_BUFFER, FS\n",
"else:\n",
" from epuck_description_py.parameters import N_BUFFER, FS\n",
"from utils.simulation import create_wideband_signal\n",
"\n",
"distances = np.arange(10, 60, step=1)\n",
"frequencies = np.fft.rfftfreq(N_BUFFER, 1 / FS)\n",
"min_freq = 100\n",
"max_freq = 5000\n",
"min_freq = 1000\n",
"max_freq = 10000\n",
"min_dist = 1\n",
"max_dist = 50\n",
"freq_start = int(min_freq / max(frequencies) * len(frequencies))\n",
Expand All @@ -63,7 +67,6 @@
"\n",
"dist = distances[dist_start:dist_end]\n",
"freq = frequencies[freq_start:freq_end]\n",
"\n",
"signal = create_wideband_signal(freq)"
]
},
Expand Down Expand Up @@ -109,7 +112,7 @@
"room.plot()\n",
"plt.xlabel(\"x [m]\")\n",
"plt.ylabel(\"y [m]\")\n",
"plt.title(\"full room\")\n"
"plt.title(\"full room\")"
]
},
{
Expand All @@ -119,9 +122,12 @@
"metadata": {},
"outputs": [],
"source": [
"signals_f = get_buffers(room, signal, n_times=2) # 1 x 4 x n_freqs\n",
"max_freq = np.inf\n",
"signals_f = get_buffers(room, signal, n_times=10) # 1 x 4 x n_freqs\n",
"signals_f = signals_f[0, chosen_mics, :].T # \n",
"nnz = np.where(np.sum(np.abs(signals_f), axis=1) > 10.0)[0]\n",
"nnz = np.where((np.sum(np.abs(signals_f), axis=1) > 20.0) & (frequencies < max_freq))[0]\n",
"\n",
"#nnz = nnz & (frequencies < max_freq)\n",
"\n",
"plt.figure()\n",
"for i in range(signals_f.shape[1]):\n",
Expand Down Expand Up @@ -218,9 +224,9 @@
"from utils.inference import get_1d_spectrum, get_angle_distribution\n",
"\n",
"angles = beamformer.theta_scan_deg\n",
"dict_ = {\"LCMV\": spec_lcmv, \n",
"dict_ = {#\"LCMV\": spec_lcmv, \n",
" \"LCMV centre\": spec_cent,\n",
" \"LCMV prop\": spec_prop,\n",
" #\"LCMV prop\": spec_prop,\n",
" \"MVDR\": spec_mvdr, \n",
" \"DAS\": spec_das,\n",
" \"GCC-PHAT\": spec_phat}\n",
Expand All @@ -240,8 +246,8 @@
" axs[i].axvline(theta * 180 / np.pi, color=f\"C{j}\", ls=\":\", label=\"canceled direction\")\n",
" axs[i].set_xlabel(\"angle [deg]\")\n",
" axs[i].legend(loc=\"lower right\")\n",
"axs[0].set_ylabel(\"normalized spectrum\")\n",
"fig.set_size_inches(15, 5)\n",
"axs[0].set_ylabel(\"normalized angular spectrum\")\n",
"fig.set_size_inches(10, 3)\n",
"save_fig(fig, f\"plots/theory/{name}source_results.pdf\")"
]
},
Expand All @@ -253,7 +259,7 @@
"outputs": [],
"source": [
"fig, ax = plt.subplots() \n",
"source_angles_deg = np.arange(91, step=30)\n",
"source_angles_deg = np.arange(91, step=10)\n",
"distance_cm = 500\n",
"for j, source_angle_deg in enumerate(source_angles_deg):\n",
" room = generate_room(distance_cm=distance_cm, source=False)\n",
Expand All @@ -270,7 +276,7 @@
" signals_f = get_buffers(room, signal, n_times=2) # 1 x 4 x n_freqs\n",
" signals_f = signals_f[0, chosen_mics, :].T\n",
" \n",
" angles, probs = get_angle_distribution(signals_f[nnz], frequencies[nnz], mics)\n",
" angles, probs = get_angle_distribution(signals_f[nnz], frequencies[nnz], mics, cancel_centre=True)\n",
" ax.plot(angles, probs, color=f\"C{j}\", label=f\"source at {source_angle_deg}$^\\circ$\")\n",
" ax.axvline(source_angle_deg, color=f\"C{j}\", ls=\":\")\n",
"ax.set_xlabel(\"angle [deg]\")\n",
Expand Down Expand Up @@ -354,30 +360,31 @@
"dist_here = np.arange(10, 60, step=10) #dist[::5]\n",
"\n",
"#azimuth_deg = 132 # normal value\n",
"azimuth_deg = 120 # normal value\n",
"azimuth_deg = 110 # normal value\n",
"\n",
"# determine signal frequency bins\n",
"room = generate_room(distance_cm=50, azimuth_deg=azimuth_deg)\n",
"signals_f = get_buffers(room, signal*100, n_times=2) # 1 x 4 x n_freqs\n",
"signals_f = signals_f[0, :, :].T\n",
"#nnz = np.where(np.sum(np.abs(signals_f), axis=1) > 100.0)[0]\n",
"nnz = np.where((frequencies > 1000) & (frequencies < 4500))[0]\n",
"nnz = np.where((frequencies > 1000) & (frequencies < 5000))[0]\n",
"\n",
"context = Context.get_crazyflie_setup(dim=2)\n",
"#mic_center = np.mean(room.mic_array.R, axis=1) # d x mics\n",
"#mics = room.mic_array.R - mic_center[:, None] # 2, 4\n",
"beamformer = BeamFormer(mic_positions=context.mics)\n",
"plt.figure()\n",
"for m in context.mics:\n",
" plt.scatter(*m)\n",
"plt.axis(\"equal\")\n",
"\n",
"plt.figure()\n",
"plt.plot(frequencies[nnz], np.abs(signals_f[nnz,0]))\n",
"#plt.figure()\n",
"#for m in context.mics:\n",
"# plt.scatter(*m)\n",
"#plt.axis(\"equal\")\n",
"\n",
"#plt.figure()\n",
"#plt.plot(frequencies[nnz], np.abs(signals_f[nnz,0]))\n",
"\n",
"noise = 0.0\n",
"\n",
"fig1, ax1 = plt.subplots()\n",
"fig2, ax2 = plt.subplots()\n",
"fig3, ax3 = plt.subplots()\n",
"fig4, ax4 = plt.subplots()\n",
"cmap = plt.get_cmap(\"viridis\", lut=len(dist_here))\n",
"for j, distance_cm in enumerate(dist_here):\n",
Expand All @@ -388,9 +395,11 @@
" buzzer = SoundSource(mic_center, signal=signal)\n",
" room.add_soundsource(buzzer)\n",
" \n",
" add_propeller_signals(room, 0.6)\n",
" signal_power = get_power(room.sources[0].signal)\n",
" noise_power = get_power(room.sources[1].signal)\n",
" if noise > 0:\n",
" add_propeller_signals(room, noise)\n",
" noise_power = get_power(room.sources[1].signal)\n",
" \n",
" #print(signal_power, noise_power)\n",
" \n",
" signals_f = get_buffers(room, signal*100, n_times=2) # 1 x 4 x n_freqs\n",
Expand All @@ -401,23 +410,38 @@
" spec = beamformer.get_lcmv_spectrum(R, frequencies_hz=frequencies[nnz], \n",
" #extra_constraints=prop_constraints,\n",
" cancel_centre=True)\n",
" \n",
" vals = get_1d_spectrum(spec)\n",
" \n",
" vals /= np.sum(vals)\n",
" ax1.plot(beamformer.theta_scan_deg, vals, color=cmap(j), label=f\"{distance_cm:.0f}cm\")\n",
" ax2.plot(frequencies[nnz], np.abs(signals_f[nnz, 0]), color=cmap(j), label=f\"{distance_cm:.0f}cm\")\n",
" #ax2.plot(frequencies[nnz], np.abs(signals_f[nnz, 0]), color=cmap(j), label=f\"{distance_cm:.0f}cm\")\n",
" \n",
" distance_estimator = DistanceEstimator()\n",
" distance_estimator.add_distributions(signals_f[nnz], frequencies[nnz], azimuth_deg=azimuth_deg)\n",
" \n",
" d_prob, prob = distance_estimator.get_distance_distribution()\n",
" ax3.plot(d_prob, prob, color=cmap(j), label=f\"{distance_cm:.0f}cm\")\n",
" #ax3.plot(d_prob, prob, color=cmap(j), label=f\"{distance_cm:.0f}cm\")\n",
" \n",
" a_prob, prob = distance_estimator.get_angle_distribution(distance_estimate_cm=distance_cm)\n",
" ax4.plot(a_prob, prob, color=cmap(j), label=f\"{distance_cm:.0f}cm\")\n",
" \n",
"ax1.legend()\n",
"ax2.legend()"
"ax4.axvline(azimuth_deg, ls=\":\", color=\"k\")\n",
"ax1.axvline(azimuth_deg, ls=\":\", color=\"k\")\n",
"ax1.set_xlabel(\"wall angle [deg]\")\n",
"ax1.set_ylabel(\"probability [-]\")\n",
"ax1.legend(title=\"wall distance\")\n",
"ax4.legend(title=\"wall distance\")\n",
"ax4.set_xlabel(\"wall angle [deg]\")\n",
"ax4.set_ylabel(\"probability [-]\")\n",
"fig1.set_size_inches(5, 3)\n",
"fig4.set_size_inches(5, 3)\n",
"ax1.set_title(\"beamforming algorithm\")\n",
"ax4.set_title(\"our magnitude (interference) algorithm\")\n",
"save_fig(fig1, f\"plots/theory/doa-phase-{str(noise).replace('.','-')}.pdf\")\n",
"save_fig(fig4, f\"plots/theory/doa-magnitude-{str(noise).replace('.','-')}.pdf\")\n",
" \n",
"#ax1.legend()\n",
"#ax2.legend()"
]
},
{
Expand Down Expand Up @@ -470,14 +494,13 @@
{
"cell_type": "code",
"execution_count": null,
"id": "90c8baf0",
"id": "dc2435f0",
"metadata": {},
"outputs": [],
"source": [
"from generate_flying_results import combine_stepper_df\n",
"stepper_df = pd.read_pickle(\"../datasets/2021_07_08_stepper_fast/all_data.pkl\")\n",
"data_df = combine_stepper_df(stepper_df, motors=\"all45000\", bin_selection=5, average=False, platform=\"crazyflie\")\n",
"print(data_df)\n",
"data_df_all = pd.read_pickle(\"../datasets/2021_10_12_flying/all_data.pkl\")\n",
"data_df = data_df_all.loc[data_df_all.appendix==\"_5\"].iloc[0]\n",
"data_df\n",
"print(data_df.stft.shape)"
]
},
Expand All @@ -489,6 +512,7 @@
"outputs": [],
"source": [
"from generate_flying_results import combine_stepper_df\n",
"print(\"WARNING: make sure to adjust utils/constants.py if you want to use this dataset\")\n",
"stepper_df = pd.read_pickle(\"../datasets/2021_07_27_epuck_wall/all_data.pkl\")\n",
"data_df = combine_stepper_df(stepper_df, motors=\"sweep_and_move\", bin_selection=0, average=False, platform=\"epuck\")\n",
"data_df.stft.shape"
Expand All @@ -497,13 +521,14 @@
{
"cell_type": "code",
"execution_count": null,
"id": "dc2435f0",
"id": "90c8baf0",
"metadata": {},
"outputs": [],
"source": [
"data_df_all = pd.read_pickle(\"../datasets/2021_10_12_flying/all_data.pkl\")\n",
"data_df = data_df_all.loc[data_df_all.appendix==\"_5\"].iloc[0]\n",
"data_df\n",
"from generate_flying_results import combine_stepper_df\n",
"stepper_df = pd.read_pickle(\"../datasets/2021_07_08_stepper_fast/all_data.pkl\")\n",
"data_df = combine_stepper_df(stepper_df, motors=\"all45000\", bin_selection=5, average=False, platform=\"crazyflie\")\n",
"print(data_df)\n",
"print(data_df.stft.shape)"
]
},
Expand Down Expand Up @@ -542,6 +567,7 @@
"outputs": [],
"source": [
"from crazyflie_demo.wall_detection import WallDetection\n",
"from crazyflie_description_py.experiments import WALL_ANGLE_DEG_STEPPER\n",
"from utils.geometry import Context\n",
"from utils.inference import get_angle_distribution\n",
"\n",
Expand All @@ -556,16 +582,19 @@
"n_positions = data_df.positions.shape[0]\n",
"\n",
"angle_estimates = []\n",
"angle_estimates_bf = []\n",
"beamform_estimates = []\n",
"distance_estimates_bf = []\n",
"distance_estimates = []\n",
"distances = []\n",
"chosen = np.arange(n_positions)\n",
"cmap = plt.get_cmap('viridis', lut=len(chosen))\n",
"\n",
"#angles_deg = [132]\n",
"WallDetection.BEAMFORM = True\n",
"#wall_detection = WallDetection(python_only=True, estimator=\"split_particle\")\n",
"print(\"initializing\")\n",
"wall_detection = WallDetection(python_only=True, estimator=\"particle\")\n",
"wall_detection.BEAMFORM = False\n",
"\n",
"wall_detection_bf = WallDetection(python_only=True, estimator=\"particle\")\n",
"wall_detection_bf.BEAMFORM = True\n",
"#wall_detection = WallDetection(python_only=True, estimator=\"moving\")\n",
"#wall_detection = WallDetection(python_only=True, estimator=\"histogram\")\n",
"\n",
Expand All @@ -578,6 +607,7 @@
"#fig.set_size_inches(10, 5)\n",
"\n",
"wall_detection.print_params()\n",
"wall_detection_bf.print_params()\n",
"\n",
"probs_mat = np.empty((0, 361))\n",
"for i_col, i in enumerate(chosen):\n",
Expand All @@ -589,7 +619,7 @@
" \n",
" if wall_detection.flight_check(position_cm):\n",
" if (i % 10) == 0:\n",
" label=\"pose {i}\"\n",
" label=f\"pose {i}\"\n",
" else:\n",
" label=None\n",
" add_point(ax_pos, position_cm, yaw_deg, cmap(i_col), label=label)\n",
Expand All @@ -603,21 +633,42 @@
" if return_dict is None:\n",
" print(\"skipping\")\n",
" continue\n",
" \n",
" \n",
" a = return_dict[\"angle_moving\"]\n",
" a_probs = return_dict[\"prob_angle_moving\"]\n",
" \n",
" d = return_dict[\"dist_moving\"]\n",
" d_probs = return_dict[\"prob_dist_moving\"]\n",
" \n",
" return_dict = wall_detection_bf.listener_callback_offline(\n",
" signals_f[:, nnz], \n",
" freqs_all[nnz], \n",
" position_cm, \n",
" yaw_deg, \n",
" timestamp=i_col)\n",
" if return_dict is None:\n",
" print(\"skipping\")\n",
" continue\n",
" \n",
" a_bf = return_dict[\"angle_moving\"]\n",
" a_probs_bf = return_dict[\"prob_angle_moving\"]\n",
" \n",
" d_bf = return_dict[\"dist_moving\"]\n",
" d_probs_bf = return_dict[\"prob_dist_moving\"]\n",
" \n",
" angles, probs = get_angle_distribution(\n",
" signals_f[:, nnz].T, freqs_all[nnz], wall_detection.estimator.context.mics.T\n",
" )\n",
" probs_mat = np.concatenate((probs_mat, probs[None, :]))\n",
" #ax.plot(angles, probs, color=cmap(i_col), label=distance)\n",
" \n",
" beamform_estimates.append(angles[np.argmax(probs)])\n",
" \n",
" angle_estimates.append(a[np.argmax(a_probs)])\n",
" distance_estimates.append(d[np.argmax(d_probs)])\n",
" angle_estimates_bf.append(a_bf[np.argmax(a_probs_bf)])\n",
" distance_estimates_bf.append(d_bf[np.argmax(d_probs_bf)])\n",
" \n",
" distances.append(distance)\n",
" \n",
"ax_pos.set_xlabel(\"x [m]\")\n",
Expand All @@ -629,14 +680,21 @@
"#ax.set_xlabel(\"angle [deg]\")\n",
"#ax.set_ylabel(\"probability [-]\")\n",
" \n",
"plt.figure()\n",
"plt.plot(distances, angle_estimates)\n",
"fig = plt.figure()\n",
"plt.plot(distances, angle_estimates, label=\"ours\")\n",
"plt.plot(distances, angle_estimates_bf, label=\"ours+beamforming\")\n",
"plt.plot(distances, beamform_estimates, label=\"beamforming\")\n",
"plt.axhline(WALL_ANGLE_DEG_STEPPER, ls=\":\", color=\"k\")\n",
"plt.legend()\n",
"plt.ylim(0, 360)\n",
"plt.xlabel(\"distance [cm]\")\n",
"plt.ylabel(\"angle estimate [deg]\")\n",
"fig.set_size_inches(10, 3)\n",
"save_fig(fig, \"plots/theory/beamform-stepper.pdf\")\n",
"\n",
"plt.figure()\n",
"plt.plot(distances, distance_estimates)\n",
"plt.plot(distances, distance_estimates, label=\"ours\")\n",
"plt.plot(distances, distance_estimates_bf, label=\"ours+beamforming\")\n",
"plt.ylim(0, 58)\n",
"plt.xlabel(\"distance [cm]\")\n",
"plt.ylabel(\"distance estimate [cm]\")"
Expand Down
Loading

0 comments on commit 9d9dfcc

Please sign in to comment.