diff --git a/.gitignore b/.gitignore index da1f2db3..19062e83 100644 --- a/.gitignore +++ b/.gitignore @@ -79,3 +79,4 @@ epuck-audio/ lost+found/ .Trash* env/ +venv/ diff --git a/README.md b/README.md index 66d97f28..6d23c47e 100644 --- a/README.md +++ b/README.md @@ -15,7 +15,7 @@ git clone --recurse-submodules https://github.com/LCAV/audioROS To install all packages contained in this repo, including dependencies, run (from the root of this repository): ``` -sudo apt-get install python3-rosdep python3-colcon-common-extensions +sudo apt install python3-rosdep python3-colcon-common-extensions sudo rosdep init cp 19-custom.list /etc/ros/rosdep/sources.list.d/ #might need sudo here rosdep update @@ -24,6 +24,13 @@ colcon build --symlink-install . install/local_setup.bash ``` +Update from May 25 2022: because of an inconsistency in matplotlib, in order to run all of the +analysis notebooks (using latex rendering), the following two +non-standard libraries had to be installed: +``` +sudo apt install cm-super dvipng +``` + ## Contents The stack is separated into the following modules: diff --git a/crazyflie-audio b/crazyflie-audio index 5471fc30..30d2daf6 160000 --- a/crazyflie-audio +++ b/crazyflie-audio @@ -1 +1 @@ -Subproject commit 5471fc306642426d39b6f1f2cdc281219920cc91 +Subproject commit 30d2daf61ea80438f33e1ca4437e5160b30c5925 diff --git a/python/2021_10_07_stepper_new_f_export_motors_binsel5_noprops_sweep_new_10.wav b/python/2021_10_07_stepper_new_f_export_motors_binsel5_noprops_sweep_new_10.wav new file mode 100644 index 00000000..89602a12 Binary files /dev/null and b/python/2021_10_07_stepper_new_f_export_motors_binsel5_noprops_sweep_new_10.wav differ diff --git a/python/Beamforming.ipynb b/python/Beamforming.ipynb new file mode 100644 index 00000000..a386d9b2 --- /dev/null +++ b/python/Beamforming.ipynb @@ -0,0 +1,751 @@ +{ + "cells": [ + { + "cell_type": "code", + "execution_count": null, + "id": "217e2a63", + "metadata": {}, + "outputs": [], + "source": [ + "import math\n", + "import sys\n", + "\n", + "import matplotlib.pylab as plt\n", + "import numpy as np\n", + "import pandas as pd\n", + "\n", + "%reload_ext autoreload\n", + "%autoreload 2\n", + "\n", + "%matplotlib inline\n", + "\n", + "from matplotlib import rcParams\n", + "import matplotlib.font_manager\n", + "\n", + "plt.rcParams.update(\n", + " {\n", + " \"figure.figsize\": (5, 3),\n", + " \"figure.max_open_warning\": False,\n", + " \"text.usetex\": True,\n", + " \"font.family\": \"DejaVu Sans\",\n", + " }\n", + ")" + ] + }, + { + "cell_type": "markdown", + "id": "1cf5bb70", + "metadata": {}, + "source": [ + "# Simulation" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "235b1395", + "metadata": {}, + "outputs": [], + "source": [ + "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 = 1000\n", + "max_freq = 10000\n", + "min_dist = 1\n", + "max_dist = 50\n", + "freq_start = int(min_freq / max(frequencies) * len(frequencies))\n", + "freq_end = int(max_freq / max(frequencies) * len(frequencies))\n", + "dist_start = int(min_dist / max(distances) * len(distances))\n", + "dist_end = int(max_dist / max(distances) * len(distances))\n", + "\n", + "dist = distances[dist_start:dist_end]\n", + "freq = frequencies[freq_start:freq_end]\n", + "signal = create_wideband_signal(freq)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "9bc27ab8", + "metadata": {}, + "outputs": [], + "source": [ + "## implement and test LMCV beamformer\n", + "from utils.simulation import generate_room, get_buffers\n", + "from utils.plotting_tools import save_fig\n", + "from audio_stack.beam_former import BeamFormer\n", + "from pyroomacoustics.soundsource import SoundSource\n", + "\n", + "chosen_mics = range(4)\n", + "#chosen_mics = (0,1)\n", + "\n", + "distance_cm = 500 # in the middle of the room\n", + "room = generate_room(distance_cm=distance_cm, source=False, chosen_mics=chosen_mics)\n", + "\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", + "print(mics.shape)\n", + "beamformer = BeamFormer(mic_positions=mics.T)\n", + "\n", + "# add a sound source at given angle from mics\n", + "source_angle_deg = 30\n", + "source = mic_center + 1 * np.r_[np.cos(source_angle_deg/180*np.pi),\n", + " np.sin(source_angle_deg/180*np.pi)]\n", + "buzzer = SoundSource(mic_center, signal=signal)\n", + "virtual_source = SoundSource(source, signal=signal*0.5)\n", + "\n", + "name=f\"double{source_angle_deg}\"\n", + "room.add_soundsource(virtual_source)\n", + "room.add_soundsource(buzzer)\n", + "\n", + "#name=\"single\"\n", + "#room.add_soundsource(virtual_source)\n", + " \n", + "fig=plt.figure()\n", + "room.plot()\n", + "plt.xlabel(\"x [m]\")\n", + "plt.ylabel(\"y [m]\")\n", + "plt.title(\"full room\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "d19713fe", + "metadata": {}, + "outputs": [], + "source": [ + "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) > 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", + " plt.plot(frequencies, np.abs(signals_f[:, i]))\n", + " plt.plot(frequencies[nnz], np.abs(signals_f[nnz, i]), color=\"k\", ls=\":\")\n", + "\n", + "fig, ax = plt.subplots()\n", + "room.plot(ax=ax)\n", + "\n", + "buzzer_constraints = []\n", + "# IMPORTANT: can only add 3 constraints because then we have a total of \n", + "# 4 (number of mics). \n", + "for i, mic in enumerate(mics.T[:2]):\n", + " theta_mic = np.arctan2(mic[1], mic[0])\n", + " buzzer_constraints.append((theta_mic, 0))\n", + " ray = 2 * np.r_[np.cos(theta_mic), np.sin(theta_mic)]\n", + " ax.plot([mic_center[0], mic_center[0] + ray[0]], \n", + " [mic_center[1], mic_center[1] + ray[1]], ls=\":\", label=\"buzzer direction\")\n", + " \n", + "prop_constraints = []\n", + "for i, mic in enumerate(mics.T[:2]):\n", + " theta_prop = np.arctan2(mic[1], mic[0]) + np.pi / 4\n", + " prop_constraints.append((theta_prop, 0))\n", + " ray = 2 * np.r_[np.cos(theta_prop), np.sin(theta_prop)]\n", + " ax.plot([mic_center[0], mic_center[0] + ray[0]], \n", + " [mic_center[1], mic_center[1] + ray[1]], ls=\":\", label=\"prop direction\")\n", + "ax.plot([mic_center[0], source[0]], \n", + " [mic_center[1], source[1]], color='k', ls=\":\", label=\"source direction\")\n", + "ax.legend()\n", + "ax.set_xlabel(\"x [m]\")\n", + "ax.set_ylabel(\"y [m]\")\n", + "mins = np.min(np.c_[room.mic_array.R,source], axis=1)*0.95\n", + "maxs = np.max(np.c_[room.mic_array.R,source], axis=1)*1.05\n", + "ax.axis(\"equal\")\n", + "ax.set_xlim(mins[0], maxs[0])\n", + "ax.set_ylim(mins[1], maxs[1])\n", + "ax.set_title(\"zoom\")\n", + "save_fig(fig, f\"plots/theory/{name}source_setup_zoom.pdf\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "f76e62d8", + "metadata": {}, + "outputs": [], + "source": [ + "# combine mics signals with beamforming. \n", + "R = beamformer.get_correlation(signals_f[nnz])\n", + "spec_lcmv = beamformer.get_lcmv_spectrum(R, frequencies_hz=frequencies[nnz], \n", + " extra_constraints=buzzer_constraints,\n", + " cancel_centre=False)\n", + "spec_cent = beamformer.get_lcmv_spectrum(R, frequencies_hz=frequencies[nnz], \n", + " extra_constraints=[], \n", + " cancel_centre=True)\n", + "spec_prop = beamformer.get_lcmv_spectrum(R, frequencies_hz=frequencies[nnz], \n", + " extra_constraints=prop_constraints, \n", + " cancel_centre=True)\n", + "spec_mvdr = beamformer.get_mvdr_spectrum(R, frequencies_hz=frequencies[nnz])\n", + "spec_das = beamformer.get_das_spectrum(R, frequencies_hz=frequencies[nnz])\n", + "spec_phat = beamformer.get_das_spectrum(R, frequencies_hz=frequencies[nnz], phat=True)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "fd9beda2", + "metadata": {}, + "outputs": [], + "source": [ + "fig, ax = plt.subplots()\n", + "im = ax.pcolormesh(beamformer.theta_scan_deg, \n", + " frequencies[nnz], \n", + " np.log10(spec_cent))\n", + "ax.axvline(source_angle_deg, color='white', ls=\":\")\n", + "plt.colorbar(im)\n", + "for c in buzzer_constraints:\n", + " if c[0] < 0:\n", + " zero_angle = c[0] * 180 / np.pi + 360\n", + " else:\n", + " zero_angle = c[0] * 180 / np.pi\n", + " ax.axvline(zero_angle, color='black', ls=\":\")\n", + "ax.set_xlabel(\"angles\")\n", + "ax.set_ylabel(\"frequencies\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "e73e88a5", + "metadata": {}, + "outputs": [], + "source": [ + "from utils.inference import get_1d_spectrum, get_angle_distribution\n", + "\n", + "angles = beamformer.theta_scan_deg\n", + "dict_ = {#\"LCMV\": spec_lcmv, \n", + " \"LCMV centre\": spec_cent,\n", + " #\"LCMV prop\": spec_prop,\n", + " \"MVDR\": spec_mvdr, \n", + " \"DAS\": spec_das,\n", + " \"GCC-PHAT\": spec_phat}\n", + "fig, axs = plt.subplots(1, len(dict_), sharey=True)\n", + "for i, (title, spec) in enumerate(dict_.items()):\n", + " #axs[i].semilogy(angles, np.sum(spec, axis=0))\n", + " \n", + " vals = get_1d_spectrum(spec)\n", + " \n", + " axs[i].plot(angles, vals)\n", + " axs[i].axvline(source_angle_deg, color=\"k\", ls=\":\", label=\"source direction\")\n", + " axs[i].set_title(title)\n", + " if title == \"LCMV\":\n", + " for j, (theta, __) in enumerate(buzzer_constraints):\n", + " if theta < 0:\n", + " theta += 2 * np.pi\n", + " 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 angular spectrum\")\n", + "fig.set_size_inches(10, 3)\n", + "save_fig(fig, f\"plots/theory/{name}source_results.pdf\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "cbe6ab12", + "metadata": {}, + "outputs": [], + "source": [ + "fig, ax = plt.subplots() \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", + " \n", + " mic_center = np.mean(room.mic_array.R, axis=1) # d x mics\n", + " # add a sound source at given angle from mics\n", + " source = mic_center + 1 * np.r_[np.cos(source_angle_deg/180*np.pi),\n", + " np.sin(source_angle_deg/180*np.pi)]\n", + " buzzer = SoundSource(mic_center, signal=signal)\n", + " virtual_source = SoundSource(source, signal=signal*0.7)\n", + " room.add_soundsource(virtual_source)\n", + " room.add_soundsource(buzzer)\n", + " \n", + " 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, 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", + "ax.set_ylabel(\"probability [-]\")\n", + "ax.legend()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "e77b62c2", + "metadata": {}, + "outputs": [], + "source": [ + "from scipy.io import wavfile \n", + "fs, prop_signal = wavfile.read(\"./2021_10_07_stepper_new_f_export_motors_binsel5_noprops_sweep_new_10.wav\")\n", + "time_range=[7, 8]\n", + "prop_signal = prop_signal[int(time_range[0]*fs):int(time_range[1]*fs)] \n", + "prop_signal = wavfile.write(\"./prop_signal.wav\", fs, prop_signal)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "2900fbb4", + "metadata": {}, + "outputs": [], + "source": [ + "def add_propeller_signals(room, amplitude_ratio=0.1):\n", + " \"\"\"\n", + " snr=0.1 means noise as 0.1 of average signal amplitude\n", + " \"\"\"\n", + " import scipy\n", + " from utils.signals import get_power\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", + " \n", + " radius_m = np.mean(np.linalg.norm(mics, axis=0))\n", + " new_radius_m = 0.05 # 5 cm\n", + " a = 45 * np.pi / 180\n", + " R = np.c_[[np.cos(a), np.sin(a)], [-np.sin(a), np.cos(a)]]\n", + " propellers = R @ mics\n", + " propellers *= new_radius_m / radius_m\n", + " propellers += mic_center[:, None]\n", + " \n", + " # Your new sampling rate\n", + " new_rate = room.fs\n", + " sampling_rate, data = wavfile.read(\"./prop_signal.wav\")\n", + " number_of_samples = round(len(data) * float(new_rate) / sampling_rate)\n", + " data = scipy.signal.resample(data, number_of_samples)\n", + " \n", + " # make sure the sound and propeller power is the same. \n", + " signal_power = get_power(room.sources[0].signal, dB=False)\n", + " noise_power = get_power(data, dB=False)\n", + " data *= np.sqrt(signal_power/noise_power)\n", + " \n", + " # adjust amplitude\n", + " data *= amplitude_ratio\n", + " \n", + " for i, p in enumerate(propellers.T):\n", + " soundsource = SoundSource(p, signal=data[i*100:])\n", + " room.add_soundsource(soundsource)\n", + " \n", + "add_propeller_signals(room)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "dd367d9f", + "metadata": { + "scrolled": false + }, + "outputs": [], + "source": [ + "## study for a close wall\n", + "from utils.estimators import DistanceEstimator\n", + "from utils.signals import get_power\n", + "from utils.geometry import Context\n", + "\n", + "dist_here = np.arange(10, 60, step=10) #dist[::5]\n", + "\n", + "#azimuth_deg = 132 # 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 < 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", + "\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", + "fig4, ax4 = plt.subplots()\n", + "cmap = plt.get_cmap(\"viridis\", lut=len(dist_here))\n", + "for j, distance_cm in enumerate(dist_here):\n", + " print(f\"distance {j+1}/{len(dist_here)}\")\n", + " room = generate_room(distance_cm=distance_cm, azimuth_deg=azimuth_deg, source=False)\n", + " \n", + " mic_center = np.mean(room.mic_array.R, axis=1) # d x mics\n", + " buzzer = SoundSource(mic_center, signal=signal)\n", + " room.add_soundsource(buzzer)\n", + " \n", + " signal_power = get_power(room.sources[0].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", + " signals_f = signals_f[0, :, :].T\n", + " \n", + " # get angle estimate with beamforming.\n", + " R = beamformer.get_correlation(signals_f[nnz])\n", + " spec = beamformer.get_lcmv_spectrum(R, frequencies_hz=frequencies[nnz], \n", + " #extra_constraints=prop_constraints,\n", + " cancel_centre=True)\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", + " \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", + " \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", + "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()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "3065a36d", + "metadata": {}, + "outputs": [], + "source": [ + "## study of the matrix after beamforming\n", + "from utils.geometry import Context\n", + "context = Context.get_platform_setup()\n", + "beamformer = BeamFormer(mic_positions=context.mics)\n", + "\n", + "cmap = plt.get_cmap(\"viridis\", lut=len(dist))\n", + "\n", + "df_matrix_beamformed = np.empty((len(frequencies[nnz]), len(dist)))\n", + "for j, distance_cm in enumerate(dist):\n", + " room = generate_room(distance_cm=distance_cm)\n", + " \n", + " signals_f = get_buffers(room, signal, n_times=2) # 1 x 4 x n_freqs\n", + " signals_f = signals_f[0, :, :].T\n", + " \n", + " # combine mics signals with beamforming. \n", + " R = beamformer.get_correlation(signals_f[nnz])\n", + " coeffs = beamformer.beamform_lcmv(R, np.pi/2, frequencies_hz=frequencies[nnz], cancel_centre=True)\n", + " slice_total = np.sum(np.multiply(coeffs, signals_f[nnz]), axis=1)\n", + " df_matrix_beamformed[:, j] = np.abs(slice_total) ** 2\n", + " \n", + "fig, ax2 = plt.subplots()\n", + "ax2.pcolorfast(dist, freq, np.log10(df_matrix_beamformed[freq_start:freq_end, :]))" + ] + }, + { + "cell_type": "markdown", + "id": "f0c4802d", + "metadata": {}, + "source": [ + "# Real data" + ] + }, + { + "cell_type": "markdown", + "id": "3d998d42", + "metadata": {}, + "source": [ + "## Stepper results" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "dc2435f0", + "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", + "print(data_df.stft.shape)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "f5f48ab4", + "metadata": {}, + "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" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "90c8baf0", + "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", + "print(data_df.stft.shape)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "b42304ab", + "metadata": {}, + "outputs": [], + "source": [ + "def plot_mics(mics):\n", + " fig_pos1, ax_pos1 = plt.subplots()\n", + " fig_pos1.set_size_inches(5, 5)\n", + " for i, mic in enumerate(mics):\n", + " ax_pos1.scatter(*mic)\n", + " ax_pos1.set_xlabel(\"x [m]\")\n", + " ax_pos1.set_ylabel(\"y [m]\")\n", + " ax_pos1.axis(\"equal\")\n", + " \n", + "def add_point(ax, position_cm, yaw_deg, color, **kwargs):\n", + " ax.scatter(position_cm[0], position_cm[1], color=color, marker='o', **kwargs)\n", + " x_dir = np.r_[np.cos(yaw_deg/180*np.pi), \n", + " np.sin(yaw_deg/180*np.pi)]\n", + " x_len = 5\n", + " ax.plot([position_cm[0], position_cm[0] + x_dir[0]*x_len], \n", + " [position_cm[1], position_cm[1] + x_dir[1]*x_len], color=color)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "38d5c1d5", + "metadata": { + "scrolled": false + }, + "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", + "np.random.seed(1) # for particle filter\n", + "\n", + "freqs_all = data_df.frequencies_matrix[0, :]\n", + "nnz = freqs_all > 0\n", + "print(freqs_all)\n", + "\n", + "context = Context.get_platform_setup()\n", + "\n", + "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", + "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", + "#plot_mics(wall_detection.estimator.context.mics)\n", + "\n", + "fig_pos, ax_pos = plt.subplots()\n", + "fig_pos.set_size_inches(5, 5)\n", + "\n", + "#fig, ax = plt.subplots()\n", + "#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", + " print(f\"{i_col+1}/{len(chosen)}\")\n", + " position_cm = data_df.positions[i, :3] * 1e2\n", + " signals_f = data_df.stft[i]\n", + " yaw_deg = data_df.positions[i, 3]\n", + " distance = -data_df.positions[i, 1] * 1e2\n", + " \n", + " if wall_detection.flight_check(position_cm):\n", + " if (i % 10) == 0:\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", + " \n", + " return_dict = wall_detection.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 = 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", + "ax_pos.set_ylabel(\"y [m]\")\n", + "ax_pos.legend()\n", + "ax_pos.axis(\"equal\")\n", + " \n", + "#ax.legend()\n", + "#ax.set_xlabel(\"angle [deg]\")\n", + "#ax.set_ylabel(\"probability [-]\")\n", + " \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, 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]\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "08ebe697", + "metadata": {}, + "outputs": [], + "source": [ + "plt.figure()\n", + "plt.pcolormesh(angles, distances, probs_mat)\n", + "plt.xlabel(\"angle [deg]\")\n", + "plt.ylabel(\"distance [cm]\")\n", + "\n", + "plt.figure()\n", + "plt.pcolormesh(angles, distances[:10], probs_mat[:10, :])\n", + "plt.xlabel(\"angle [deg]\")\n", + "plt.ylabel(\"distance [cm]\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "4ad9494c", + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3 (ipykernel)", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.8.10" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/python/DistanceFlying.ipynb b/python/DistanceFlying.ipynb index 3bc7c19e..c2a723c8 100644 --- a/python/DistanceFlying.ipynb +++ b/python/DistanceFlying.ipynb @@ -4,9 +4,9 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "# Distance: Flying Experiments\n", + "# Flying Experiments \n", "\n", - "Detect the distance to the wall while flying and playing sweeps." + "Detect and localize the wall while flying and playing sweeps." ] }, { @@ -18,8 +18,6 @@ "import math\n", "import sys\n", "\n", - "import IPython\n", - "import IPython.display as ipd\n", "import matplotlib.pylab as plt\n", "import numpy as np\n", "import pandas as pd\n", @@ -28,16 +26,66 @@ "%autoreload 2\n", "\n", "%matplotlib inline\n", - "#%matplotlib notebook\n", "\n", "from matplotlib import rcParams\n", "\n", "rcParams[\"figure.max_open_warning\"] = False\n", "rcParams[\"font.family\"] = 'DejaVu Sans'\n", "rcParams[\"font.size\"] = 14\n", - "rcParams[\"text.usetex\"] = True\n", - "#import rclpy\n", - "#rclpy.init()" + "rcParams[\"text.usetex\"] = True" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# check that interpolation works as expected\n", + "from numpy import array, float32\n", + "freqs = array([2500., 2562., 2625., 2687., 2750., 2812., 2875., 2937., 3000.,\n", + " 3062., 3125., 3187., 3250., 3312., 3375., 3437., 3562., 3625.,\n", + " 3687., 3750., 3812., 3875., 3937., 4000., 4062., 4125., 4187.,\n", + " 4250., 4312., 4375., 4437., 4500.], dtype=float32) \n", + "freqs_interp = array([2500., 2562., 2625., 2687., 2750., 2812., 2875., 2937., 3000.,\n", + " 3062., 3125., 3187., 3250., 3312., 3375., 3437., 3499., 3562.,\n", + " 3625., 3687., 3750., 3812., 3875., 3937., 4000., 4062., 4125.,\n", + " 4187., 4250., 4312., 4375., 4437., 4500.], dtype=float32)\n", + "\n", + "plt.figure()\n", + "plt.scatter(freqs_interp, [0.5] * len(freqs_interp), label=\"interpolated\")\n", + "plt.scatter(freqs, [0.6] * len(freqs), label=\"original\")\n", + "plt.legend()\n", + "plt.ylim(0, 1)\n", + "\n", + "freqs = array([3140., 3187., 3234., 3328., 3375., 3468., 3515., 3562., 3984.,\n", + " 4031., 4078., 4171., 4218., 4265., 4359., 4406., 4500., 4546.],\n", + " dtype=float32)\n", + "freqs_interp = array([3140., 3187., 3234., 3280., 3328., 3375., 3421., 3468., 3515.,\n", + " 3562., 3984., 4031., 4078., 4124., 4171., 4218., 4265., 4311.,\n", + " 4359., 4406., 4452., 4500., 4546.], dtype=float32) \n", + "\n", + "plt.figure()\n", + "plt.scatter(freqs_interp, [0.5] * len(freqs_interp), label=\"interpolated\")\n", + "plt.scatter(freqs, [0.6] * len(freqs), label=\"original\")\n", + "plt.legend()\n", + "plt.ylim(0, 1)\n", + "\n", + "freqs = array([1156., 1281., 1406., 1546., 1671., 1796., 1921., 2062., 2187.,\n", + " 2312., 2437., 2562., 2718., 2843., 2953., 3109., 3234., 3359.,\n", + " 3468., 3625., 3734., 3859., 3984., 4125., 4265., 4421., 4515.,\n", + " 4687., 4781., 4875., 5078., 5187.], dtype=float32) \n", + "freqs_interp = array([1156., 1281., 1406., 1546., 1671., 1796., 1921., 2062., 2187.,\n", + " 2312., 2437., 2562., 2718., 2843., 2953., 3109., 3234., 3359.,\n", + " 3468., 3625., 3734., 3859., 3984., 4125., 4265., 4421., 4515.,\n", + " 4687., 4781., 4875., 5000., 5078., 5187.], dtype=float32)\n", + "\n", + "\n", + "plt.figure()\n", + "plt.scatter(freqs_interp, [0.5] * len(freqs_interp), label=\"interpolated\")\n", + "plt.scatter(freqs, [0.6] * len(freqs), label=\"original\")\n", + "plt.legend()\n", + "plt.ylim(0, 1)" ] }, { @@ -47,7 +95,7 @@ "outputs": [], "source": [ "def get_title(row, ignore=[]):\n", - " categories = row.index.drop([\"matrix distances\", \"matrix angles\", \"distances_cm\", \"angles_deg\"]).values\n", + " categories = row.index.drop([\"matrix distances\", \"matrix angles\", \"distances_cm\", \"angles_deg\", \"average_time\"]).values\n", " title = ''\n", " for param_name in row.index:\n", " if param_name in ignore:\n", @@ -66,41 +114,103 @@ "metadata": {}, "outputs": [], "source": [ - "def get_calib_function(calib=\"stepper\"):\n", - " from utils.calibration import get_calibration_function_median\n", - " from utils.calibration import get_calibration_function_moving\n", - " if calib == \"flying\":\n", - " print(\"using flying\")\n", - " calib_function_median, freqs = get_calibration_function_moving(\n", - " \"2021_10_12_flying\", motors=\"linear_buzzer_cont\", fit_one_gain=False,\n", - " #\"2021_07_14_flying\", motors=\"linear_buzzer_cont\", fit_one_gain=False,\n", - " appendix_list=[\"_new3\", \"_new4\", \"_new6\"], \n", - " )\n", - " elif calib == \"stepper\":\n", - " print(\"using stepper dataset\")\n", - " calib_function_median, freqs = get_calibration_function_median(\n", - " #\"2021_07_08_stepper_fast\",\n", - " \"2021_10_07_stepper_new_f\",\n", - " motors=\"all45000\",\n", - " mic_type=\"audio_deck\",\n", - " snr=5,\n", - " fit_one_gain=False,\n", - " )\n", - " return calib_function_median\n", - "\n", - "def get_estimates_here(results_matrix, xvalues, method=\"mean\"):\n", - " if method == \"argmax\":\n", + "def get_estimates_here(results_matrix, xvalues, method=\"max\"):\n", + " if method in [\"argmax\", \"max\"]:\n", " estimates = xvalues[np.argmax(results_matrix, axis=0)].astype(float)\n", " elif method == \"mean\":\n", " estimates = np.empty(results_matrix.shape[1])\n", " for col in range(results_matrix.shape[1]):\n", " mean = np.average(xvalues, weights=results_matrix[:, col], axis=0)\n", " estimates[col] = mean\n", + " else:\n", + " raise ValueError(method)\n", " valid = np.any(results_matrix > 0, axis=0)\n", " estimates[~valid] = np.nan\n", " return estimates" ] }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "def plot_matrix(yvalues, results_matrix, gt_values=None, \n", + " xvalues=None, no_deco=False, angles=False, \n", + " log=True, n_calib=0):\n", + " from utils.plotting_tools import add_colorbar, pcolorfast_custom, FIGSIZE\n", + " from copy import copy\n", + " \n", + " cmap = copy(plt.get_cmap())\n", + " cmap.set_bad('gray')\n", + " if xvalues is None:\n", + " xvalues = np.arange(results_matrix.shape[1])\n", + " xlabel = \"index [-]\"\n", + " else:\n", + " xlabel = \"time [s]\"\n", + " \n", + " estimates = get_estimates_here(results_matrix, yvalues)\n", + " fig, ax = plt.subplots()\n", + " fig.set_size_inches(2 * FIGSIZE, FIGSIZE)\n", + " \n", + " if log:\n", + " plot_matrix = copy(results_matrix)\n", + " plot_matrix[plot_matrix == 0] = np.nan\n", + " plot_matrix[plot_matrix > 0] = np.log10(plot_matrix[plot_matrix>0])\n", + " else:\n", + " plot_matrix = results_matrix\n", + " \n", + " im = ax.pcolormesh(xvalues, yvalues, plot_matrix, cmap=cmap)\n", + " \n", + " if gt_values is not None:\n", + " ax.plot(\n", + " xvalues,\n", + " gt_values,\n", + " color=\"black\",\n", + " label=\"\\\\textbf{ground truth}\",\n", + " marker='x'\n", + " )\n", + " ax.set_ylim(min(yvalues), max(yvalues))\n", + " ax.plot(\n", + " xvalues,\n", + " estimates,\n", + " color=\"white\",\n", + " label=\"\\\\textbf{estimates}\",\n", + " marker='o',\n", + " ls='-'\n", + " )\n", + " if no_deco:\n", + " ax.set_yticks([])\n", + " ax.set_xticks([])\n", + " else:\n", + " add_colorbar(fig, ax, im, title=\"log-probability\" if log else \"probability\")\n", + " ax.set_ylabel(\"estimated angle [deg]\" if angles else \"estimated distance [cm]\")\n", + " ax.set_xlabel(xlabel)\n", + " \n", + " leg = ax.legend(framealpha=0, loc='upper right')\n", + " for text in leg.get_texts():\n", + " plt.setp(text, color='w', weight='bold')\n", + " #ax.set_xticks(xticks, labels=xticklabels)\n", + " #ax.set_yticks(yticks, labels=yticks)\n", + " #n_xticks = 5\n", + " #step = len(xvalues) // n_xticks\n", + " #ax.set_xticks(np.round(xvalues[::step], 1))\n", + " #ax.set_xticklabels(np.round(xvalues[::step], 1))\n", + " \n", + " alpha = 0.7\n", + " if n_calib:\n", + " from matplotlib.patches import Rectangle\n", + " width = xvalues[n_calib] - xvalues[0]\n", + " height= max(yvalues) - min(yvalues)\n", + " diff_x = xvalues[1] - xvalues[0]\n", + " xy = (min(xvalues) - diff_x/2, min(yvalues))\n", + " rect = Rectangle(xy, width, height, facecolor='white', alpha=alpha)\n", + " ax.add_patch(rect)\n", + " #if not no_deco:\n", + " # ax.text(xy[0] + width/2, xy[1]+height/2, \"used for\\ncalibration\", fontdict={'color':'black', 'ha':'center', 'va':'bottom'})\n", + " return fig, ax" + ] + }, { "cell_type": "code", "execution_count": null, @@ -140,7 +250,7 @@ "source": [ "## 1. Single wall approach experiments\n", "\n", - "### Qualitative evaluation" + "### 1.1 Qualitative evaluation" ] }, { @@ -153,37 +263,37 @@ "from crazyflie_description_py.parameters import FLYING_HEIGHT_CM\n", "from generate_classifier_results import get_groundtruth_distances, WALLS_DICT\n", "\n", + "from utils.constants import SPEED_OF_SOUND\n", "from utils.plotting_tools import FIGSIZE, save_fig\n", "\n", - "#exp_name=\"2021_10_12_flying\"; motors=\"linear_buzzer_cont\"; \n", + "exp_name=\"2021_10_12_flying\"; motors=\"linear_buzzer_cont\"; \n", "#appendix=\"_new3\" # best\n", "#appendix=\"_new6\" # too fast, otherwise good\n", "#appendix=\"_new10\" # okay-ish\n", "#appendix=\"_new12\"\n", - "\n", "#appendix=\"_new4\" # frequencies wrong\n", "#appendix=\"_new7\" # frequencies wrong\n", "#appendix=\"_new8\" # frequencies wrong\n", - "#appendix=\"_1\" # very good\n", + "appendix=\"_1\" # very good\n", "#appendix=\"_2\" # not good\n", "#appendix=\"_3\" # okay\n", "#appendix=\"_4\" # good\n", "#appendix=\"_5\"A # not good\n", "#appendix=\"_6\"\n", "\n", - "exp_name=\"2022_01_27_demo\"; motors=\"live\"; \n", - "appendix = \"test4\"\n", + "#exp_name=\"2022_01_27_demo\"; motors=\"live\"; \n", + "#appendix = \"test4\"\n", + "\n", + "#exp_name=\"2021_07_27_epuck_wall\"; motors=\"sweep_and_move\"; appendix = \"\"\n", + "#exp_name=\"2021_07_08_stepper_fast\"; motors=\"all45000\"; appendix = \"\"\n", "\n", "results_df = pd.read_pickle(f\"../datasets/{exp_name}/all_data.pkl\")\n", - "print(results_df.appendix.unique())\n", - "print(results_df.motors.unique())\n", "row = results_df.loc[\n", " (results_df.appendix == appendix) & (results_df.motors == motors), :\n", "].iloc[0]\n", "\n", "bad_freqs = []\n", "n_calib = 5\n", - "figsize = 3\n", "\n", "flying = row.positions[:, 2] > FLYING_HEIGHT_CM * 1e-2\n", "positions_cm = row.positions[flying, :2] * 1e2\n", @@ -196,9 +306,10 @@ "distances = get_groundtruth_distances(exp_name, appendix, flying=True)\n", "\n", "from utils.constants import SPEED_OF_SOUND\n", - "print(freqs[1]-freqs[0])\n", - "print(SPEED_OF_SOUND)\n", - "print(SPEED_OF_SOUND / (4 * (freqs[1]-freqs[0])))\n", + "delta_f = np.min(np.diff(freqs))\n", + "print(\"minimum difference\", delta_f)\n", + "max_d = SPEED_OF_SOUND / (4 * delta_f)\n", + "print(\"maximum distance:\", max_d)\n", "\n", "# calibrate and plot calibration\n", "calib_on_the_fly = np.median(magnitudes[:n_calib, :, :], axis=0)\n", @@ -219,161 +330,16 @@ "\n", "# plot positions\n", "fig, ax = plt.subplots()\n", - "fig.set_size_inches(figsize, figsize)\n", + "fig.set_size_inches(3, 3)\n", "plot_positions(ax, positions_cm, WALLS_DICT[exp_name])\n", - "plot_name = f\"plots/experiments/{exp_name}{appendix}_positions.pdf\"\n", - "#save_fig(fig, plot_name)" + "plot_name = f\"plots/experiments/{exp_name}{appendix}_positions.pdf\"" ] }, { - "cell_type": "code", - "execution_count": null, + "cell_type": "markdown", "metadata": {}, - "outputs": [], "source": [ - "import itertools\n", - "import time\n", - "\n", - "from utils.plotting_tools import pcolorfast_custom, plot_performance\n", - "from utils.inference import Inference\n", - "from utils.estimators import DistanceEstimator\n", - "from utils.moving_estimators import MovingEstimator\n", - "from utils.particle_estimators import ParticleEstimator\n", - "\n", - "# for live analysis\n", - "from crazyflie_demo.wall_detection import DISTANCES_CM, ANGLES_DEG\n", - "distances_cm = DISTANCES_CM\n", - "angles_deg = ANGLES_DEG\n", - "\n", - "# for offline analysis\n", - "#distances_cm = np.arange(100, step=1)\n", - "#angles_deg = np.arange(360, step=2)\n", - "\n", - "#particle_method = \"gaussian\"\n", - "particle_method = \"histogram\"\n", - "n_calib = 5\n", - "n_window = 3\n", - "simplify_angles = False\n", - "\n", - "calib_method_list = [\"on_the_fly\"]#, [\"stepper\"]\n", - "mics_list = [[0, 1, 2, 3]] #, [[1, 3], [0, 2], [0, 1, 3], [0, 1, 2, 3]]\n", - "algorithm_list = [\"bayes\"]#, \"cost\"]\n", - "no_deco = False\n", - "errors_df = pd.DataFrame(\n", - " columns=[\"algorithm\", \"mics\", \"calib_method\", \"estimates\", \"distances\", \"appendix\", \"time\"]\n", - ")\n", - "\n", - "inf_machine = Inference()\n", - "inf_machine.add_geometry([distances_cm[0], distances_cm[-1]], WALL_ANGLE_DEG)\n", - "\n", - "runtimes_inference = []\n", - "\n", - "for calib_method, mics, algorithm in itertools.product(calib_method_list, mics_list, algorithm_list):\n", - " if calib_method == \"on_the_fly\":\n", - " calib_values = calib_on_the_fly\n", - " else:\n", - " calib_function = get_calib_function(calib_method)\n", - " calib_values = calib_function(freqs)\n", - " magnitudes_calib = magnitudes / calib_values\n", - "\n", - " mics_str = str(mics).replace(\" \", \"\")\n", - " plot_name = f\"plots/experiments/{exp_name}{appendix}_{algorithm}_{calib_method}_{mics_str}\"\n", - "\n", - " results_matrix = np.full((len(distances_cm), n_points), np.nan)\n", - " results_matrix_angles = np.full((len(angles_deg), n_points), np.nan)\n", - "\n", - " moving_estimator = MovingEstimator(n_window=n_window, distances_cm=distances_cm, angles_deg=angles_deg)\n", - " results_matrix_angles_moving = np.full((len(angles_deg), n_points), np.nan)\n", - " results_matrix_moving = np.full((len(distances_cm), n_points), np.nan)\n", - " \n", - " particle_estimator = ParticleEstimator(n_particles=100, global_=False, distances_cm=distances_cm, angles_deg=angles_deg)\n", - " results_matrix_angles_part = np.full((len(angles_deg), n_points), np.nan)\n", - " results_matrix_part = np.full((len(distances_cm), n_points), np.nan)\n", - "\n", - " time_moving = 0\n", - " time_single = 0\n", - " for i in range(n_points):\n", - " print(f\"{i}/{n_points}\")\n", - "\n", - " # important to reinitialize!\n", - " distance_estimator = DistanceEstimator(distances_cm=distances_cm, angles_deg=angles_deg)\n", - "\n", - " inf_machine.add_data(magnitudes_calib[i], freqs)\n", - " inf_machine.filter_out_freqs(freq_ranges=bad_freqs)\n", - "\n", - " t1 = time.time()\n", - " diff_moving = {}\n", - " t1 = time.time()\n", - " for mic_idx in mics:\n", - " dist, prob_mic, diff = inf_machine.do_inference(algorithm, mic_idx)\n", - " distance_estimator.add_distribution(diff * 1e-2, prob_mic, mic_idx)\n", - " diff_moving[mic_idx] = (diff, prob_mic)\n", - " runtimes_inference.append(time.time() - t1)\n", - "\n", - " __, prob = distance_estimator.get_distance_distribution(verbose=False, angle_deg=WALL_ANGLE_DEG)\n", - " time_single += int(1000*(time.time() - t1))\n", - "\n", - " moving_estimator.add_distributions(\n", - " diff_moving, position_cm=positions_cm[i], rot_deg=yaws_deg[i]\n", - " )\n", - " t1 = time.time()\n", - " particle_estimator.add_distributions(\n", - " diff_moving, position_cm=positions_cm[i], rot_deg=yaws_deg[i]\n", - " )\n", - " runtimes_inference[-1] += time.time() - t1\n", - " \n", - " __, prob_moving, __, prob_angle_moving = moving_estimator.get_distributions(simplify_angles=simplify_angles)\n", - " time_moving += int(1000*(time.time() - t1))\n", - " \n", - " t1 = time.time()\n", - " __, prob_part, __, prob_angle_part = particle_estimator.get_distributions(simplify_angles=simplify_angles, method=particle_method)\n", - " runtimes_inference[-1] += time.time() - t1\n", - " \n", - " dist_est_cm = distances_cm[np.argmax(prob_moving)]\n", - " __, prob_angle = distance_estimator.get_angle_distribution(\n", - " distance_estimate_cm=dist_est_cm\n", - " )\n", - "\n", - " results_matrix[:, i] = prob\n", - " results_matrix_angles[:, i] = prob_angle\n", - " results_matrix_moving[:, i] = prob_moving \n", - " results_matrix_angles_moving[:, i] = prob_angle_moving \n", - " results_matrix_part[:, i] = prob_part\n", - " results_matrix_angles_part[:, i] = prob_angle_part\n", - "\n", - " continue\n", - " # DEBUGGING ONLY\n", - " dist, *_ = moving_estimator.get_joint_distribution(simplify_angles=simplify_angles)\n", - " fig, ax = plt.subplots()\n", - " pcolorfast_custom(ax, distances_cm, angles_deg, dist)\n", - " ax.axvline(distances[i], color='white', ls=':')\n", - " ax.axhline(90, color='white', ls=':')\n", - " ax.set_xlabel('distances [cm]')\n", - " ax.set_ylabel('angles [deg]')\n", - " ax.set_title(\"joint distribution\")\n", - "\n", - " d_estimates = get_estimates_here(results_matrix, distances_cm)\n", - " errors_df.loc[len(errors_df), :] = {\n", - " \"algorithm\": algorithm,\n", - " \"mics\": mics_str,\n", - " \"calib_method\": calib_method,\n", - " \"estimates\": d_estimates,\n", - " \"distances\": distances,\n", - " \"appendix\": appendix,\n", - " \"time\": time_single / n_points \n", - " }\n", - "\n", - " d_estimates_moving = get_estimates_here(results_matrix_moving, distances_cm)\n", - " errors_df.loc[len(errors_df), :] = {\n", - " \"algorithm\": algorithm + f\"_win{n_window}\",\n", - " \"mics\": mics_str,\n", - " \"calib_method\": calib_method,\n", - " \"estimates\": d_estimates_moving,\n", - " \"distances\": distances,\n", - " \"appendix\": appendix,\n", - " \"time\": time_moving / n_points \n", - " }\n", - "print(\"done\")" + "### 1.2 Quantitative evaluation" ] }, { @@ -382,78 +348,21 @@ "metadata": {}, "outputs": [], "source": [ - "def plot_matrix(yvalues, results_matrix, gt_values, \n", - " xvalues=None, no_deco=False, angles=False, \n", - " log=True, n_calib=0):\n", - " from utils.plotting_tools import add_colorbar, pcolorfast_custom, FIGSIZE\n", - " \n", - " cmap = plt.get_cmap().copy() \n", - " cmap.set_bad('gray')\n", - " if xvalues is None:\n", - " xvalues = np.arange(results_matrix.shape[1])\n", - " xlabel = \"index [-]\"\n", - " else:\n", - " xlabel = \"time [s]\"\n", - " \n", - " estimates = get_estimates_here(results_matrix, yvalues)\n", - " fig, ax = plt.subplots()\n", - " fig.set_size_inches(2 * FIGSIZE, FIGSIZE)\n", - " \n", - " if log:\n", - " plot_matrix = results_matrix.copy()\n", - " plot_matrix[plot_matrix == 0] = np.nan\n", - " plot_matrix[plot_matrix > 0] = np.log10(plot_matrix[plot_matrix>0])\n", - " else:\n", - " plot_matrix = results_matrix\n", - " \n", - " im = ax.pcolormesh(xvalues, yvalues, plot_matrix, cmap=cmap)\n", - " \n", - " ax.plot(\n", - " xvalues,\n", - " gt_values,\n", - " color=\"black\",\n", - " label=\"\\\\textbf{ground truth}\",\n", - " marker='x'\n", - " )\n", - " ax.set_ylim(min(yvalues), max(yvalues))\n", - " ax.plot(\n", - " xvalues,\n", - " estimates,\n", - " color=\"white\",\n", - " label=\"\\\\textbf{estimates}\",\n", - " marker='o',\n", - " ls='-'\n", - " )\n", - " if no_deco:\n", - " ax.set_yticks([])\n", - " ax.set_xticks([])\n", - " else:\n", - " add_colorbar(fig, ax, im, title=\"log-probability\" if log else \"probability\")\n", - " ax.set_ylabel(\"estimated angle [deg]\" if angles else \"estimated distance [cm]\")\n", - " ax.set_xlabel(xlabel)\n", - " \n", - " leg = ax.legend(framealpha=0, loc='upper right')\n", - " for text in leg.get_texts():\n", - " plt.setp(text, color='w', weight='bold')\n", - " #ax.set_xticks(xticks, labels=xticklabels)\n", - " #ax.set_yticks(yticks, labels=yticks)\n", - " #n_xticks = 5\n", - " #step = len(xvalues) // n_xticks\n", - " #ax.set_xticks(np.round(xvalues[::step], 1))\n", - " #ax.set_xticklabels(np.round(xvalues[::step], 1))\n", - " \n", - " alpha = 0.7\n", - " if n_calib:\n", - " from matplotlib.patches import Rectangle\n", - " width = xvalues[n_calib] - xvalues[0]\n", - " height= max(yvalues) - min(yvalues)\n", - " diff_x = xvalues[1] - xvalues[0]\n", - " xy = (min(xvalues) - diff_x/2, min(yvalues))\n", - " rect = Rectangle(xy, width, height, facecolor='white', alpha=alpha)\n", - " ax.add_patch(rect)\n", - " #if not no_deco:\n", - " # ax.text(xy[0] + width/2, xy[1]+height/2, \"used for\\ncalibration\", fontdict={'color':'black', 'ha':'center', 'va':'bottom'})\n", - " return fig, ax" + "mics_beam = np.array([[-0.011, 0.048, 0.019, 0.019], [-0.018, -0.017, 0.014, -0.048]])\n", + "room_mics = np.array([[9.418, 9.417, 9.386, 9.448], [3.489, 3.548, 3.519, 3.519]])\n", + "\n", + "plt.figure()\n", + "for i, m in enumerate(mics_beam.T):\n", + " plt.scatter(*m, label=f\"mic{i}\")\n", + "plt.axis(\"equal\")\n", + "plt.legend()\n", + "\n", + "plt.figure()\n", + "#for i, m in enumerate(mics_beam.T):\n", + "for i, m in enumerate(room_mics.T):\n", + " plt.scatter(*m, label=f\"mic{i}\")\n", + "plt.axis(\"equal\")\n", + "plt.legend()" ] }, { @@ -464,288 +373,268 @@ }, "outputs": [], "source": [ - "fig_size = (6, 4)\n", - "fig, ax = plot_matrix(distances_cm, results_matrix, distances, xvalues=times, no_deco=no_deco, \n", - " n_calib=n_calib)\n", - "fig.set_size_inches(*fig_size)\n", - "plot_name = f\"plots/experiments/{exp_name}{appendix}_matrix.pdf\"\n", - "#save_fig(fig, plot_name)\n", - "\n", - "fig, ax = plot_matrix(distances_cm, results_matrix_moving, distances, xvalues=times,no_deco=no_deco,\n", - " n_calib=n_calib)\n", - "fig.set_size_inches(*fig_size)\n", - "plot_name = f\"plots/experiments/{exp_name}{appendix}_matrix_hist.pdf\"\n", - "#save_fig(fig, plot_name)\n", - "fig, ax = plot_matrix(distances_cm, results_matrix_part, distances, xvalues=times, no_deco=no_deco,\n", - " n_calib=n_calib)\n", - "fig.set_size_inches(*fig_size)\n", - "plot_name = f\"plots/experiments/{exp_name}{appendix}_matrix_part.pdf\"\n", - "#save_fig(fig, plot_name)\n", - "\n", - "gt_angles = np.full(len(distances), 90)\n", - "fig, ax = plot_matrix(angles_deg, results_matrix_angles, gt_angles, xvalues=times,\n", - " no_deco=no_deco, angles=True, \n", - " n_calib=n_calib)\n", - "fig.set_size_inches(*fig_size)\n", - "plot_name = f\"plots/experiments/{exp_name}{appendix}_matrix_angle.pdf\"\n", - "#save_fig(fig, plot_name)\n", - "fig, ax = plot_matrix(np.array(angles_deg), results_matrix_angles_moving, gt_angles, xvalues=times,\n", - " no_deco=no_deco, log=False, angles=True,\n", - " n_calib=n_calib)\n", - "fig.set_size_inches(*fig_size)\n", - "plot_name = f\"plots/experiments/{exp_name}{appendix}_matrix_hist_angle.pdf\"\n", - "#save_fig(fig, plot_name)\n", - "fig, ax = plot_matrix(np.array(angles_deg), results_matrix_angles_part, gt_angles, xvalues=times,\n", - " no_deco=no_deco, log=False, angles=True,\n", - " n_calib=n_calib)\n", - "fig.set_size_inches(*fig_size)\n", - "plot_name = f\"plots/experiments/{exp_name}{appendix}_matrix_part_angle.pdf\"\n", - "#save_fig(fig, plot_name)" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "## calculate cdfs to find best parameters" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "ignore = [\"calibration name\", \"outlier factor\"]\n", + "from generate_classifier_results import get_groundtruth_distances\n", + "from plot_helpers import ls, labels, add_double_legend\n", + "from generate_filtering_results import angle_error\n", + "np.random.seed(2)\n", "\n", - "#estimator = \"moving\"\n", - "estimator = \"moving\"\n", - "df_mat = pd.read_pickle(\"results/flying_results_matrices_{estimator}.pkl\") \n", - "ignore_here = [\"n window\", \"mask bad\", \"simplify angles\", \"relative std\"]\n", - "df_mat = df_mat[df_mat[\"relative std\"] == 0.0]\n", + "D_RANGE = [7, 80]\n", + "PLOT_D_MAX = 30\n", + "\n", + "no_deco = True\n", + "fig_size = (6, 3)\n", "\n", + "chosen_calibration_param = 0.3\n", + "simplify_angles = False\n", "fig_size_cdf = (10, 4)\n", - "gt_distances, gt_angles = get_groundtruth_distances(exp_name, appendix, flying=True, angles=True)\n", - "for (simp, relative_std, n_window), rows_mat in df_mat.groupby([\"simplify angles\", \"relative std\", \"n window\"]):\n", - " fig, axs = plt.subplots(1, 2)\n", - " fig.set_size_inches(*fig_size_cdf)\n", - " fig.suptitle(f\"{estimator} $N_w={n_window}$, angles simplified {simp}\")\n", - " \n", - " for i, row in rows_mat.iterrows():\n", - " label = get_title(row, ignore=ignore + ignore_here)\n", - " \n", - " n_calib_here = int(np.floor(row[\"calibration param\"]))\n", - " d_here = row[\"distances_cm\"]\n", - " a_here = row[\"angles_deg\"]\n", - " matrix_distances = row[\"matrix distances\"][0]\n", - " matrix_angles = row[\"matrix angles\"][0]\n", "\n", - " d_estimates = get_estimates_here(matrix_distances, d_here)\n", + "exp_name=\"2021_10_12_flying\"\n", + "name = \"flying\"; appendices = [\"_new3\", \"_new6\"] + [f\"_{i}\" for i in range(1, 7)] \n", "\n", - " cdf_distances = np.sort(np.abs(d_estimates[n_calib:] - gt_distances[n_calib:]))\n", - " y = np.linspace(0, 1, len(cdf_distances))\n", - " axs[0].plot(cdf_distances, y, label=label)\n", + "#exp_name=\"2021_07_08_stepper_fast\"\n", + "#name = \"stepper\"; appendices=[\"\"]\n", "\n", - " if not row[\"simplify angles\"]:\n", - " a_estimates = get_estimates_here(matrix_angles, a_here)\n", - " cdf_angles = np.sort(np.abs(a_estimates - gt_angles))\n", - " y = np.linspace(0, 1, len(cdf_angles))\n", - " axs[1].plot(cdf_angles, y, label=label)\n", - " axs[0].grid(True)\n", - " axs[1].grid(True)\n", - " axs[0].set_xlabel(\"error distance [cm]\")\n", - " axs[1].set_xlabel(\"error angle [deg]\")\n", - " axs[0].set_ylabel(\"cdf [-]\")\n", - " axs[1].set_ylabel(\"cdf [-]\")\n", - " axs[0].set_xlim(0, 80)\n", - " axs[1].set_xlim(0, 180)\n", - " axs[0].legend(loc=\"lower right\")" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "estimator = \"particle\"\n", - "df_mat = pd.read_pickle(f\"results/flying_results_matrices_{estimator}.pkl\") \n", - "ignore_here = [\"n window\", \"relative std\", \"mask bad\", \"simplify angles\"]\n", - "\n", - "fig_size = (10, 4)\n", - "gt_distances, gt_angles = get_groundtruth_distances(exp_name, appendix, flying=True, angles=True)\n", - "for (simp), rows_mat in df_mat.groupby([\"simplify angles\"]):\n", - " fig, axs = plt.subplots(1, 2)\n", - " fig.set_size_inches(*fig_size)\n", - " fig.suptitle(f\"{estimator} $N_w={n_window}$, angles simplified {simp}\")\n", - " \n", - " for i, row in rows_mat.iterrows():\n", - " label = get_title(row, ignore=ignore + ignore_here)\n", + "#exp_name=\"2021_07_27_epuck_wall\"\n", + "#name = \"epuck\"; appendices=[\"\"]\n", + "\n", + "#fig_cdf.suptitle(f\"angles simplified {simplify_angles}\")\n", + "\n", + "beamform = False\n", + "discretization = \"fine\"\n", + "\n", + "color = {f\"exp{a}\": \"C0\" if \"new\" in a else \"C1\" for i, a in enumerate(appendices) }\n", + "color[\"random\"] = \"k\"\n", + "color[\"fixed\"] = \"k\"\n", + "\n", + "#for estimator in [\"moving\"]: #[\"particle\"]:#, \"moving\"]:\n", + "for estimator in [\"particle\"]:\n", + "#for estimator in [\"histogram\", \"moving\", \"particle\"]:#, \"moving\"]:\n", + " fig_cdf, axs_cdf = plt.subplots(1, 2, sharey=True)\n", + " fig_cdf.set_size_inches(*fig_size_cdf)\n", + " for i_app, appendix in enumerate(appendices):\n", + " \n", + " # flow deck was drifting a lot for experiments downstairs (carpet)\n", + " # so we use the wall as a reference for ground truth distance (the drone always hit the wall)\n", + " if not \"_new\" in appendix:\n", + " correct = True\n", + " else:\n", + " correct = False\n", + " \n", + " gt_distances, gt_angles = get_groundtruth_distances(exp_name, appendix, \n", + " flying=True, angles=True, correct=correct)\n", " \n", - " n_calib_here = int(np.floor(row[\"calibration param\"]))\n", - " d_here = row[\"distances_cm\"]\n", - " a_here = row[\"angles_deg\"]\n", - " matrix_distances = row[\"matrix distances\"][0]\n", - " matrix_angles = row[\"matrix angles\"][0]\n", + " if beamform:\n", + " fname = f\"results/{name}_results_matrices_{estimator}{appendix}_beamform_new.pkl\"\n", + " else:\n", + " fname = f\"results/{name}_results_matrices_{estimator}{appendix}_new.pkl\"\n", + " df_mat = pd.read_pickle(fname) \n", + " print(\"read\", fname)\n", + " df_here = df_mat[\n", + " (df_mat[\"calibration param\"]==chosen_calibration_param) & \n", + " (df_mat[\"simplify angles\"]==simplify_angles) \n", + " ]\n", + " for i, row in df_here.iterrows():\n", + " if estimator == \"particle\":\n", + " method = \"particle\"\n", + " elif estimator == \"moving\":\n", + " method = f\"histogram {row['n window']}\"\n", + " else:\n", + " method = \"histogram\"\n", + " \n", + " if appendix == appendices[0]:\n", + " label = labels[method]\n", + " #print(\"label\", label)\n", + " else:\n", + " label = None\n", + " \n", + " n_calib_here = int(np.floor(row[\"calibration param\"]))\n", + " d_here = row[\"distances_cm\"]\n", + " a_here = row[\"angles_deg\"]\n", + " matrix_distances = row[\"matrix distances\"]\n", + " matrix_angles = row[\"matrix angles\"]\n", "\n", - " d_estimates = get_estimates_here(matrix_distances, d_here)\n", + " d_estimates = get_estimates_here(matrix_distances, d_here)\n", "\n", - " cdf_distances = np.sort(np.abs(d_estimates[n_calib:] - gt_distances[n_calib:]))\n", - " y = np.linspace(0, 1, len(cdf_distances))\n", - " axs[0].plot(cdf_distances, y, label=label)\n", + " cdf_distances = np.sort(np.abs(d_estimates[n_calib:] - gt_distances[n_calib:]))\n", + " y = np.linspace(0, 1, len(cdf_distances))\n", + " axs_cdf[0].plot(cdf_distances, y, ls=ls[method], label=label, color=color[f\"exp{appendix}\"])\n", "\n", - " if not row[\"simplify angles\"]:\n", - " a_estimates = get_estimates_here(matrix_angles, a_here)\n", - " cdf_angles = np.sort(np.abs(a_estimates - gt_angles))\n", - " y = np.linspace(0, 1, len(cdf_angles))\n", - " axs[1].plot(cdf_angles, y, label=label)\n", - " axs[0].grid(True)\n", - " axs[1].grid(True)\n", - " axs[0].set_xlabel(\"error distance [cm]\")\n", - " axs[1].set_xlabel(\"error angle [deg]\")\n", - " axs[0].set_ylabel(\"cdf [-]\")\n", - " axs[1].set_ylabel(\"cdf [-]\")\n", - " axs[0].set_xlim(0, 80)\n", - " axs[1].set_xlim(0, 180)\n", - " axs[0].legend(loc=\"lower right\")" + " if not row[\"simplify angles\"]:\n", + " a_estimates = get_estimates_here(matrix_angles, a_here)\n", + " \n", + " error = angle_error(a_estimates, gt_angles)\n", + " cdf_angles = np.sort(np.abs(error))\n", + " y = np.linspace(0, 1, len(cdf_angles))\n", + " axs_cdf[1].plot(cdf_angles, y, label=label, ls=ls[method], color=color[f\"exp{appendix}\"])\n", + " \n", + " fig, ax = plot_matrix(d_here, matrix_distances, #xvalues=times, \n", + " gt_values=gt_distances,\n", + " no_deco=no_deco, n_calib=n_calib_here)\n", + " ax.set_title(f\" exp{appendix} {estimator}\".replace(\"_\", \"\\\\_\"))\n", + " fig.set_size_inches(*fig_size)\n", + " \n", + " fig, ax = plot_matrix(a_here, matrix_angles, \n", + " gt_values=gt_angles, \n", + " no_deco=no_deco, n_calib=n_calib_here)\n", + " ax.set_title(f\" exp{appendix} {estimator}\".replace(\"_\", \"\\\\_\"))\n", + " fig.set_size_inches(*fig_size)\n", + "\n", + " lines = []; leg_labels = []\n", + " line, = axs_cdf[1].plot([], [], color=\"C0\")\n", + " lines.append(line); leg_labels.append(\"dataset \\nglass wall\")\n", + " line, = axs_cdf[1].plot([], [], color=\"C1\")\n", + " lines.append(line); leg_labels.append(\"dataset \\nwhiteboard\")\n", + " \n", + " # generate random\n", + " d_random = np.random.choice(range(*D_RANGE), size=len(gt_distances))\n", + " a_random = np.random.choice(range(360), size=len(gt_angles))\n", + " axs_cdf[0].plot(sorted(np.abs(d_random-gt_distances)), np.linspace(0, 1, len(gt_distances)), color=color[\"random\"])\n", + " line, = axs_cdf[1].plot(sorted(np.abs(angle_error(a_random, gt_angles))), np.linspace(0, 1, len(gt_angles)), color=color[\"random\"])\n", + " lines.append(line); leg_labels.append(\"random\")\n", + " \n", + " # generate fixed\n", + " d_fixed = (D_RANGE[1]-D_RANGE[0]) / 2\n", + " a_fixed = gt_angles[0] + 180 # \n", + " line, = axs_cdf[0].plot(sorted(np.abs(d_fixed-gt_distances)), np.linspace(0, 1, len(gt_distances)), ls=\":\", color=color[\"fixed\"])\n", + " lines.append(line); leg_labels.append(\"fixed\")\n", + "\n", + " axs_cdf[1].plot(sorted(np.abs(angle_error(a_fixed, gt_angles))), np.linspace(0, 1, len(gt_angles)), ls=\":\", color=color[\"fixed\"])\n", + " axs_cdf[1].legend(lines, leg_labels, title=\"used data\", loc=\"upper left\", bbox_to_anchor=[1.0, 0.8])\n", + "\n", + " axs_cdf[0].grid(True)\n", + " axs_cdf[1].grid(True)\n", + " axs_cdf[0].set_xlabel(\"absolute error [cm]\")\n", + " axs_cdf[1].set_xlabel(\"absolute error [deg]\")\n", + " axs_cdf[0].set_title(\"distance estimation\")\n", + " axs_cdf[1].set_title(\"angle estimation\")\n", + " axs_cdf[0].set_ylabel(\"cdf [-]\")\n", + " #axs_cdf[1].set_ylabel(\"cdf [-]\")\n", + " axs_cdf[0].set_xlim(0, PLOT_D_MAX)\n", + " axs_cdf[1].set_xlim(0, 200)\n", + " fig_cdf.set_size_inches(*fig_size)\n", + " fig_cdf.subplots_adjust(wspace=0.1)\n", + " if beamform:\n", + " save_fig(fig_cdf, f\"plots/experiments/{exp_name}_{discretization.replace(' ','_')}_{estimator}_beamform_cdfs.pdf\")\n", + " else:\n", + " save_fig(fig_cdf, f\"plots/experiments/{exp_name}_{discretization.replace(' ','_')}_{estimator}_cdfs.pdf\")" ] }, { - "cell_type": "markdown", + "cell_type": "code", + "execution_count": null, "metadata": {}, + "outputs": [], "source": [ - "# plot heatmaps and cdfs" + "### Comparison of datasets" ] }, { "cell_type": "code", "execution_count": null, "metadata": { - "scrolled": true + "scrolled": false }, "outputs": [], "source": [ - "from plot_helpers import ls, labels, add_double_legend\n", - "from generate_filtering_results import angle_error\n", - "np.random.seed(2)\n", - "\n", - "print(ls)\n", - "chosen_calibration_param = 0.3\n", - "simplify_angles = False\n", - "fig_size_cdf = (10, 4)\n", - "\n", - "fig_cdf, axs_cdf = plt.subplots(1, 2)\n", - "fig_cdf.set_size_inches(*fig_size_cdf)\n", - "#fig_cdf.suptitle(f\"angles simplified {simplify_angles}\")\n", - "\n", - "discretization = \"fine\"\n", + "no_deco=False\n", "\n", - "#appendices = [\"_new3\", \"_new6\"] + [f\"_{i}\" for i in range(1, 3)] \n", - "appendices = [\"_new3\", \"_new6\"] + [f\"_{i}\" for i in range(1, 7)] \n", - "#appendices = [f\"_{i}\" for i in range(1, 7)] \n", - "color = {f\"exp{a}\": \"C0\" if \"new\" in a else \"C1\" for i, a in enumerate(appendices) }\n", + "color = {\"epuck\": \"C0\", \"stepper\": \"C1\"}\n", "color[\"random\"] = \"k\"\n", "color[\"fixed\"] = \"k\"\n", "\n", - "for estimator in [\"particle\"]:#, \"moving\"]:\n", - " for i_app, appendix in enumerate(appendices):\n", + "estimator = \"particle\"\n", + "appendix = \"\"\n", + "\n", + "fig_cdf, axs_cdf = plt.subplots(1, 2, sharey=True)\n", + "fig_cdf.set_size_inches(*fig_size_cdf)\n", + "\n", + "linestyles = {\n", + " \"pyroom\": \":\",\n", + " \"\": \"-\"\n", + "}\n", + "names = {\n", + " \"pyroom\": \" simulated\",\n", + " \"\": \"\"\n", + "}\n", + "rename = {\n", + " \"stepper\": \"drone\",\n", + " \"epuck\": \"e-puck\",\n", + "}\n", + "\n", + "fig_hor, (ax_dist, ax_ang) = plt.subplots(2, 1, sharex=True)\n", + "\n", + "for exp_name in [\"2021_07_08_stepper_fast\", \"2021_07_27_epuck_wall\"]:\n", + " if exp_name == \"2021_07_27_epuck_wall\":\n", + " print(\"epuck\")\n", + " from epuck_description_py.experiments import DISTANCES_CM, WALL_ANGLE_DEG_STEPPER\n", + " gt_distances = DISTANCES_CM\n", + " gt_angles = np.full(len(DISTANCES_CM), WALL_ANGLE_DEG_STEPPER)\n", + " name = \"epuck\"\n", + " elif exp_name == \"2021_07_08_stepper_fast\":\n", + " print(\"stepper\")\n", + " from crazyflie_description_py.experiments import DISTANCES_CM, WALL_ANGLE_DEG_STEPPER\n", + " gt_distances = DISTANCES_CM\n", + " gt_angles = np.full(len(DISTANCES_CM), WALL_ANGLE_DEG_STEPPER)\n", + " name = \"stepper\"\n", " \n", - " # flow deck was drifting a lot for experiments downstairs (carpet)\n", - " # so we use the wall as a reference for ground truth distance (the drone always hit the wall)\n", - " if not \"_new\" in appendix:\n", - " correct = True\n", - " else:\n", - " correct = False\n", - " gt_distances, gt_angles = get_groundtruth_distances(exp_name, appendix, flying=True, angles=True, correct=True)\n", - " df_mat = pd.read_pickle(f\"results/flying_results_matrices_{estimator}{appendix}.pkl\") \n", - " if estimator == \"particle\":\n", - " relative_std = 0.0\n", - " #relative_std = 1.0 # no effect\n", - " if estimator == \"moving\":\n", - " relative_std = 0.0 # no effect\n", - " df_here = df_mat[\n", - " (df_mat[\"calibration param\"]==chosen_calibration_param) & \n", - " (df_mat[\"simplify angles\"]==simplify_angles) & \n", - " (df_mat[\"relative std\"]==relative_std)\n", - " ]\n", + " for simulate in [\"\", \"pyroom\"]:\n", + " fname = f\"results/{name}_results_matrices_{estimator}{appendix}{simulate}_new.pkl\"\n", + " df_here = pd.read_pickle(fname) \n", " for i, row in df_here.iterrows():\n", - " if estimator == \"particle\":\n", - " method = \"particle\"\n", - " else:\n", - " method = f\"histogram {row['n window']}\"\n", - " \n", - " if appendix == appendices[0]:\n", - " label = labels[method]\n", - " else:\n", - " label = None\n", - " \n", - " n_calib_here = int(np.floor(row[\"calibration param\"]))\n", " d_here = row[\"distances_cm\"]\n", " a_here = row[\"angles_deg\"]\n", - " matrix_distances = row[\"matrix distances\"][0]\n", - " matrix_angles = row[\"matrix angles\"][0]\n", + " matrix_distances = row[\"matrix distances\"]\n", + " matrix_angles = row[\"matrix angles\"]\n", "\n", " d_estimates = get_estimates_here(matrix_distances, d_here)\n", + " gt_distances_here = row.gt_distances_cm\n", + " gt_angles_here = row.gt_angles_deg\n", "\n", - " cdf_distances = np.sort(np.abs(d_estimates[n_calib:] - gt_distances[n_calib:]))\n", + " d_errors = d_estimates - gt_distances_here\n", + " cdf_distances = np.sort(np.abs(d_errors))\n", " y = np.linspace(0, 1, len(cdf_distances))\n", - " axs_cdf[0].plot(cdf_distances, y, ls=ls[method], label=label, color=color[f\"exp{appendix}\"])\n", + " label=rename[name] if simulate == \"\" else None\n", + " axs_cdf[0].plot(cdf_distances, y, ls=linestyles[simulate], label=label, color=color[name])\n", + " ax_dist.plot(gt_distances_here, d_errors, color=color[name], ls=linestyles[simulate])\n", + "\n", + " a_estimates = get_estimates_here(matrix_angles, a_here)\n", + "\n", + " a_errors = angle_error(a_estimates, gt_angles_here)\n", + " \n", + " ax_ang.plot(gt_distances_here, a_errors, color=color[name], ls=linestyles[simulate])\n", + " \n", + " cdf_angles = np.sort(np.abs(a_errors))\n", + " y = np.linspace(0, 1, len(cdf_angles))\n", + " label=rename[name] if simulate == \"\" else None\n", + " axs_cdf[1].plot(cdf_angles, y, ls=linestyles[simulate], label=label, color=color[name])\n", "\n", - " if not row[\"simplify angles\"]:\n", - " a_estimates = get_estimates_here(matrix_angles, a_here)\n", - " \n", - " error = angle_error(a_estimates, gt_angles)\n", - " cdf_angles = np.sort(np.abs(error))\n", - " y = np.linspace(0, 1, len(cdf_angles))\n", - " axs_cdf[1].plot(cdf_angles, y, label=label, ls=ls[method], color=color[f\"exp{appendix}\"])\n", - " \n", " fig, ax = plot_matrix(d_here, matrix_distances, #xvalues=times, \n", - " gt_values=gt_distances,\n", - " no_deco=no_deco, n_calib=n_calib_here)\n", - " ax.set_title(f\" exp{appendix}\")\n", + " gt_values=gt_distances_here,\n", + " no_deco=no_deco)\n", + " ax.set_title(name)\n", " fig.set_size_inches(*fig_size)\n", - " #save_fig(fig, f\"plots/experiments/{exp_name}{appendix}_{estimator}_distance_{i}_{simplify_angles}.pdf\")\n", "\n", - " if (estimator == \"moving\") and row[\"simplify angles\"]:\n", - " continue\n", - " \n", - " continue\n", - " fig, ax = plot_matrix(a_here, matrix_angles, #xvalues=times, \n", - " gt_values = gt_angles,\n", - " no_deco=no_deco, n_calib=n_calib_here)\n", - " ax.set_title(f\" exp{appendix}\")\n", - " #ax.set_title(label)\n", + " fig, ax = plot_matrix(a_here, matrix_angles, \n", + " gt_values=gt_angles_here, \n", + " no_deco=no_deco)\n", + " ax.set_title(name)\n", " fig.set_size_inches(*fig_size)\n", - " save_fig(fig, f\"plots/experiments/{exp_name}{appendix}_{estimator}_angle_{i}_{simplify_angles}.pdf\")\n", - " \n", - "figsize = 2\n", - "leg = axs_cdf[1].legend(title=\"filter\", loc=\"lower left\", bbox_to_anchor=[1.0, 0.5])\n", - "\n", - "print(d_here)\n", - "d_random = np.random.choice(range(7, 81), size=len(gt_distances))\n", - "a_random = np.random.choice(range(360), size=len(gt_angles))\n", - "\n", - "lines = []; labels = []\n", - "\n", - "axs_cdf[0].plot(sorted(np.abs(d_random-gt_distances)), np.linspace(0, 1, len(gt_distances)), color=color[\"random\"])\n", - "line, = axs_cdf[1].plot(sorted(np.abs(angle_error(a_random, gt_angles))), np.linspace(0, 1, len(gt_angles)), color=color[\"random\"])\n", - "lines.append(line); labels.append(\"random\")\n", - "\n", - "d_fixed = (80 - 7) / 2\n", - "a_fixed = 90 + 180\n", - "a_random = np.random.choice(range(360), size=len(gt_angles))\n", - "line, = axs_cdf[0].plot(sorted(np.abs(d_fixed-gt_distances)), np.linspace(0, 1, len(gt_distances)), ls=\":\", color=color[\"fixed\"])\n", - "lines.append(line); labels.append(\"fixed\")\n", - "\n", - "axs_cdf[1].plot(sorted(np.abs(angle_error(a_fixed, gt_angles))), np.linspace(0, 1, len(gt_angles)), ls=\":\", color=color[\"fixed\"])\n", - "line, = axs_cdf[1].plot([], [], color=\"C0\")\n", - "lines.append(line); labels.append(\"dataset glass wall\")\n", - "line, = axs_cdf[1].plot([], [], color=\"C1\")\n", - "lines.append(line); labels.append(\"dataset whiteboard\")\n", - "axs_cdf[1].legend(lines, labels, title=\"used data\", loc=\"upper left\", bbox_to_anchor=[1.0, 0.5])\n", - "axs_cdf[1].add_artist(leg)\n", - "\n", - "#add_double_legend(axs_cdf[1], color=color)\n", + "\n", + "# generate random\n", + "d_random = np.random.choice(range(7, 81), size=len(gt_distances_here))\n", + "a_random = np.random.choice(range(360), size=len(gt_angles_here))\n", + "axs_cdf[0].plot(sorted(np.abs(d_random-gt_distances_here)), np.linspace(0, 1, len(gt_distances_here)), color=color[\"random\"], label=\"random\")\n", + "axs_cdf[1].plot(sorted(np.abs(angle_error(a_random, gt_angles_here))), np.linspace(0, 1, len(gt_angles_here)), color=color[\"random\"], label=\"random\")\n", + "\n", + "# generate fixed\n", + "d_fixed = (50-7)/2\n", + "a_fixed = gt_angles[0] + 180\n", + "axs_cdf[0].plot(sorted(np.abs(d_fixed-gt_distances_here)), np.linspace(0, 1, len(gt_distances_here)), ls=\":\", color=color[\"fixed\"], label=\"fixed\")\n", + "axs_cdf[1].plot(sorted(np.abs(angle_error(a_fixed, gt_angles_here))), np.linspace(0, 1, len(gt_angles_here)), ls=\":\", color=color[\"fixed\"], label=\"fixed\")\n", + "\n", + "#axs_cdf[1].legend(title=\"used data\", loc=\"upper left\", bbox_to_anchor=[1.0, 0.75])\n", + "axs_cdf[1].legend(title=\"used data\", loc=\"lower right\", framealpha=1.0)\n", + "\n", "axs_cdf[0].grid(True)\n", "axs_cdf[1].grid(True)\n", "axs_cdf[0].set_xlabel(\"absolute error [cm]\")\n", @@ -753,19 +642,37 @@ "axs_cdf[0].set_title(\"distance estimation\")\n", "axs_cdf[1].set_title(\"angle estimation\")\n", "axs_cdf[0].set_ylabel(\"cdf [-]\")\n", - "axs_cdf[1].set_ylabel(\"cdf [-]\")\n", - "axs_cdf[0].set_xlim(0, 30)\n", + "#axs_cdf[1].set_ylabel(\"cdf [-]\")\n", + "axs_cdf[0].set_xlim(0, PLOT_D_MAX)\n", "axs_cdf[1].set_xlim(0, 200)\n", - "fig_cdf.set_size_inches(8, 4)\n", - "fig_cdf.subplots_adjust(wspace=0.3)\n", - "save_fig(fig_cdf, f\"plots/experiments/{exp_name}_{discretization.replace(' ','_')}_cdfs.pdf\")" + "fig_cdf.set_size_inches(5, 4)\n", + "#fig_cdf.subplots_adjust(wspace=0.3)\n", + "\n", + "fig_hor.set_size_inches(3, 4)\n", + "\n", + "ax_ang.set_xlabel(\"distance [cm]\")\n", + "#ax_dist.set_xlabel(\"distance [cm]\")\n", + "ax_dist.set_title(\"error evolution\")\n", + "\n", + "ax_ang.set_ylabel(\"error [deg]\")\n", + "ax_dist.set_ylabel(\"error [cm]\")\n", + "ax_ang.yaxis.set_label_coords(-0.23,0.5)\n", + "ax_dist.yaxis.set_label_coords(-0.23,0.5)\n", + "ax_ang.grid()\n", + "ax_dist.grid()\n", + "if beamform:\n", + " save_fig(fig_cdf, f\"plots/experiments/comparison_{estimator}_beamform_cdfs.pdf\")\n", + " save_fig(fig_hor, f\"plots/experiments/comparison_{estimator}_beamform.pdf\")\n", + "else:\n", + " save_fig(fig_cdf, f\"plots/experiments/comparison_{estimator}_cdfs.pdf\")\n", + " save_fig(fig_hor, f\"plots/experiments/comparison_{estimator}.pdf\")" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ - "### Factor graph test" + "### 1.3 Factor graph inference" ] }, { @@ -780,9 +687,6 @@ "import gtsam\n", "from audio_gtsam.wall_backend import WallBackend, add_decorations\n", "\n", - "from moviepy.editor import VideoClip\n", - "from moviepy.video.io.bindings import mplfig_to_npimage\n", - "\n", "X = gtsam.symbol_shorthand.X\n", "P = gtsam.symbol_shorthand.P\n", "\n", @@ -790,12 +694,10 @@ "yaw_start = np.pi \n", "\n", "use_groundtruth = False\n", - "#use_groundtruth = True \n", "\n", "dist_matrix = results_matrix_moving\n", "angle_matrix = results_matrix_angles_moving\n", "n_estimates = 1\n", - "#print(angle_matrix.shape, dist_matrix.shape)\n", "\n", "d_estimates = []\n", "d_gt = []\n", @@ -808,7 +710,10 @@ " if i < n_calib:\n", " continue\n", " \n", - " position_cm = np.r_[positions_cm[i, :], 0.0]\n", + " if len(positions_cm[i, :]) < 3:\n", + " position_cm = np.r_[positions_cm[i, :], 0.0]\n", + " else:\n", + " position_cm = positions_cm[i, :]\n", " yaw = yaws_deg[i] / 180 * np.pi\n", " pose_factor = wall_backend.add_pose(r_world=position_cm * 1e-2, yaw=yaw, verbose=False)\n", " \n", @@ -834,7 +739,6 @@ " distance_estimate = wall_backend.get_distance_estimate()\n", " d_estimates.append(distance_estimate * 1e2 if distance_estimate else np.nan)\n", " d_gt.append(distance_gt if distance_estimate else np.nan)\n", - " #print(f\"time with isam={wall_backend.use_isam}: {(time.time() - t1)*1e3:.0f}ms\")\n", " \n", " wall_backend.plot(fig, ax, n_poses=20)\n", "add_decorations(fig, ax, n_poses=20)\n", @@ -847,18 +751,7 @@ " \"distances\": d_gt,\n", " \"appendix\": appendix,\n", " \"time\": time_moving / n_points \n", - "}\n", - " \n", - "\n", - "#duration = 20\n", - "#animation = VideoClip(make_frame, duration=duration, )\n", - "#animation.ipython_display(fps = 20, loop = True, autoplay = True)\n", - "\n", - "#plot_projections(wall_backend.all_initial_estimates, axis_length=0.2, ls=\":\", perspective=False, side=True)\n", - "#plt.show()\n", - "#wall_backend.get_results()\n", - "#plot_projections(wall_backend.result, axis_length=0.1, perspective=False, side=True)\n", - "#plt.show() " + "}" ] }, { @@ -903,10 +796,7 @@ "print(\"available appendices:\", results_df.appendix.values)\n", "\n", "appendix = \"test4\" # works well\n", - "#appendix = \"test3\" # works ok\n", - "#appendix = \"test2\" # doesn't detect\n", - "#appendix = \"test1\" # crashes\n", - "#appendix = \"test0\" # works ok" + "#appendix = \"test3\" # works ok" ] }, { @@ -923,7 +813,6 @@ "\n", "positions_cm = row.positions[:, :3] * 1e2\n", "flying = (positions_cm[:, 2] > FLYING_HEIGHT_CM) & (positions_cm[:, 2] < 100) & (np.abs(positions_cm[:, 1]) < 100) & (np.abs(positions_cm[:, 0]) < 50)\n", - "print(\"not flying:\", np.where(~flying)[0])\n", "positions_cm = positions_cm[flying, :]\n", "yaws_deg = row.positions[flying, 3]\n", "freqs = row.frequencies_matrix[0, :]\n", @@ -948,96 +837,6 @@ "metadata": {}, "outputs": [], "source": [ - "from generate_classifier_results import DIST_THRESH, STD_THRESH, TAIL_THRESH\n", - "\n", - "def plot_distance_matrix(ax, matrix, distances_cm=None):\n", - " from utils.plotting_tools import pcolorfast_custom\n", - " from copy import deepcopy\n", - " \n", - " if distances_cm is None:\n", - " distances_cm = wall_detection.estimator.distances_cm\n", - "\n", - " cmap = plt.get_cmap(\"gray\").copy()\n", - " alpha = 0.8\n", - " cmap.set_bad((1 - alpha, 1 - alpha, 1 - alpha))\n", - " log_matrix = deepcopy(matrix)\n", - " log_matrix[matrix > 0] = np.log10(matrix[matrix > 0])\n", - " log_matrix[matrix <= 0] = np.nan\n", - " pcolorfast_custom(\n", - " ax,\n", - " np.arange(log_matrix.shape[1]),\n", - " distances_cm,\n", - " log_matrix,\n", - " n_xticks=12,\n", - " n_yticks=4,\n", - " cmap=cmap,\n", - " alpha=alpha,\n", - " )\n", - " ax.set_ylabel(\"distance [cm]\")\n", - "\n", - "\n", - "def plot_groundtruth_d(ax, color=\"white\", **kwargs):\n", - " ax.plot(distances_walls, color=color, **kwargs)\n", - "\n", - "def plot_distance_estimates(ax, distance_estimates, label=None, **kwargs):\n", - " ax.plot(distance_estimates, label=label, **kwargs)\n", - " ax.set_ylabel(\"distance [cm]\")\n", - " ax.axhline(DIST_THRESH, color=\"k\", ls=\"--\")\n", - "\n", - "\n", - "def plot_std_estimates(ax, std_estimates, label=None, **kwargs):\n", - " ax.plot(std_estimates, label=label, **kwargs)\n", - " ax.set_ylabel(\"standard deviation [cm]\")\n", - " ax.axhline(STD_THRESH, color=\"k\", ls=\"--\")\n", - " ax.set_yscale(\"log\")\n", - " ax.grid(True, which=\"both\")\n", - "\n", - "\n", - "def plot_tail(ax, matrix, **kwargs):\n", - " tail = np.log10(np.mean(matrix[-3:, :], axis=0))\n", - " ax.plot(tail, **kwargs)\n", - " ax.set_ylabel(\"tail probability\")\n", - " ax.grid(True)\n", - " ax.axhline(TAIL_THRESH, color=\"k\", ls=\"--\")\n", - "\n", - "\n", - "def plot_angles(ax, matrix, **kwargs):\n", - " matrix_norm = matrix / np.sum(matrix, axis=0)[None, :]\n", - " for i in range(matrix_norm.shape[0]):\n", - " angle = ANGLES_DEG[i]\n", - " ax.plot(matrix_norm[i, :], label=f\"wall at {angle}\", **kwargs)\n", - " ax.legend(loc=\"upper right\")\n", - " ax.grid(True)\n", - " ax.set_ylabel(\"angle probability\")\n", - " \n", - "def plot_fg_estimates(ax, d_estimates, colors):\n", - " ax.scatter(np.arange(len(d_estimates)), d_estimates, color=colors)\n", - "\n", - "def plot_all():\n", - " fig, axs = plt.subplots(4, 1, sharex=True)\n", - " fig.set_size_inches(20, 30)\n", - "\n", - " plot_distance_matrix(axs[0], results_matrix_moving)\n", - " ymin, ymax = axs[0].get_ylim()\n", - " plot_groundtruth_d(axs[0], color=\"white\")\n", - " plot_distance_estimates(axs[0], distance_estimates, label=\"chosen method\")\n", - " \n", - " plot_fg_estimates(axs[0], fg_dist_estimates, fg_colors)\n", - " axs[0].set_ylim(ymin, ymax)\n", - " \n", - " plot_groundtruth_d(axs[1], color=\"black\")\n", - " plot_distance_estimates(axs[1], distance_estimates, label=\"chosen method\")\n", - " plot_fg_estimates(axs[1], fg_dist_estimates, fg_colors)\n", - " axs[1].set_ylim(5, 100)\n", - " axs[1].grid(True)\n", - "\n", - " plot_std_estimates(axs[2], std_estimates, label=\"chosen method\")\n", - "\n", - " #plot_tail(axs[3], results_matrix_moving)\n", - " if not WallDetection.SIMPLIFY_ANGLES:\n", - " plot_angles(axs[3], results_matrix_angles)\n", - " return fig, axs\n", - "\n", "def plot_groundtruth(ax, positions_cm, walls, n_poses=20):\n", " from audio_gtsam.wall_backend import plot_wall, add_decorations\n", " cmap = plt.get_cmap('inferno', n_timesteps) \n", @@ -1052,8 +851,7 @@ " np.sin(wall_angle / 180 * np.pi)\n", " ]\n", " arrow, line = plot_wall(wall_distance*1e-2, normal, ax, plane_index=i, label=f\"real $\\\\pi^{{({i})}}$\", ls=\":\", lw=3.0)\n", - " add_decorations(fig, ax, n_poses=n_poses)\n", - " " + " add_decorations(fig, ax, n_poses=n_poses)" ] }, { @@ -1071,6 +869,8 @@ "from crazyflie_demo.wall_detection import WallDetection\n", "from crazyflie_description_py.experiments import WALL_ANGLE_DEG\n", "\n", + "import time\n", + "\n", "estimation_method = \"mean\"\n", "estimator = \"moving\"\n", "\n", @@ -1101,7 +901,7 @@ " if i % 20 == 0:\n", " print(f\"{i+1}/{n_timesteps}\")\n", " res = wall_detection.listener_callback_offline(\n", - " signals_f[i].T, freqs, positions_cm[i], yaws_deg[i], timestamp=times[i]*1e3\n", + " signals_f[i], freqs, positions_cm[i], yaws_deg[i], timestamp=times[i]*1e3\n", " )\n", " #fig, ax = plt.subplots()\n", " #ax.pcolorfast(wall_detection.calibration_data[0, :, :])\n", @@ -1126,7 +926,9 @@ " #angle_local = wall_detection.estimator.get_local_forward_angle()\n", " #angles_forward.append(angle_local)\n", " \n", - " __, __, prob_moving_dist, prob_moving_angle = res\n", + " prob_moving_dist = res[\"prob_dist_moving\"]\n", + " prob_moving_angle = res[\"prob_angle_moving\"]\n", + " #__, __, prob_moving_dist, prob_moving_angle = res\n", " \n", " results_matrix_moving[:, i] = prob_moving_dist\n", " \n", @@ -1163,38 +965,107 @@ ] }, { - "cell_type": "code", - "execution_count": null, + "cell_type": "markdown", "metadata": {}, - "outputs": [], "source": [ - "plt.figure()\n", - "plt.plot(np.array(runtimes_fg) * 1e3)\n", - "plt.axhline(np.mean(runtimes_fg) * 1e3)\n", - "plt.xlabel(\"time index [-]\")\n", - "plt.ylabel(\"time [ms]\")\n", - "plt.figure()\n", - "plt.plot(np.array(runtimes_inference) * 1e3)\n", - "plt.axhline(np.mean(runtimes_inference) * 1e3)\n", - "plt.xlabel(\"time index [-]\")\n", - "plt.ylabel(\"time [ms]\")" + "# Sandbox" ] }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [] - }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ - "fig, axs = plot_all()\n", - "save_fig(fig, f'plots/experiments/{exp_name}{appendix}_distributions.pdf')" + "from generate_classifier_results import DIST_THRESH, STD_THRESH, TAIL_THRESH\n", + "\n", + "def plot_distance_matrix(ax, matrix, distances_cm=None):\n", + " from utils.plotting_tools import pcolorfast_custom\n", + " from copy import deepcopy\n", + " \n", + " if distances_cm is None:\n", + " distances_cm = wall_detection.estimator.distances_cm\n", + "\n", + " cmap = plt.get_cmap(\"gray\")\n", + " alpha = 0.8\n", + " cmap.set_bad((1 - alpha, 1 - alpha, 1 - alpha))\n", + " log_matrix = deepcopy(matrix)\n", + " log_matrix[matrix > 0] = np.log10(matrix[matrix > 0])\n", + " log_matrix[matrix <= 0] = np.nan\n", + " pcolorfast_custom(\n", + " ax,\n", + " np.arange(log_matrix.shape[1]),\n", + " distances_cm,\n", + " log_matrix,\n", + " n_xticks=12,\n", + " n_yticks=4,\n", + " cmap=cmap,\n", + " alpha=alpha,\n", + " )\n", + " ax.set_ylabel(\"distance [cm]\")\n", + "\n", + "\n", + "def plot_groundtruth_d(ax, color=\"white\", **kwargs):\n", + " ax.plot(distances_walls, color=color, **kwargs)\n", + "\n", + "def plot_distance_estimates(ax, distance_estimates, label=None, **kwargs):\n", + " ax.plot(distance_estimates, label=label, **kwargs)\n", + " ax.set_ylabel(\"distance [cm]\")\n", + " ax.axhline(DIST_THRESH, color=\"k\", ls=\"--\")\n", + "\n", + "\n", + "def plot_std_estimates(ax, std_estimates, label=None, **kwargs):\n", + " ax.plot(std_estimates, label=label, **kwargs)\n", + " ax.set_ylabel(\"standard deviation [cm]\")\n", + " ax.axhline(STD_THRESH, color=\"k\", ls=\"--\")\n", + " ax.set_yscale(\"log\")\n", + " ax.grid(True, which=\"both\")\n", + "\n", + "\n", + "def plot_tail(ax, matrix, **kwargs):\n", + " tail = np.log10(np.mean(matrix[-3:, :], axis=0))\n", + " ax.plot(tail, **kwargs)\n", + " ax.set_ylabel(\"tail probability\")\n", + " ax.grid(True)\n", + " ax.axhline(TAIL_THRESH, color=\"k\", ls=\"--\")\n", + "\n", + "\n", + "def plot_angles(ax, matrix, **kwargs):\n", + " matrix_norm = matrix / np.sum(matrix, axis=0)[None, :]\n", + " for i in range(matrix_norm.shape[0]):\n", + " angle = ANGLES_DEG[i]\n", + " ax.plot(matrix_norm[i, :], label=f\"wall at {angle}\", **kwargs)\n", + " ax.legend(loc=\"upper right\")\n", + " ax.grid(True)\n", + " ax.set_ylabel(\"angle probability\")\n", + " \n", + "def plot_fg_estimates(ax, d_estimates, colors):\n", + " ax.scatter(np.arange(len(d_estimates)), d_estimates, color=colors)\n", + "\n", + "def plot_all():\n", + " fig, axs = plt.subplots(4, 1, sharex=True)\n", + " fig.set_size_inches(20, 30)\n", + "\n", + " plot_distance_matrix(axs[0], results_matrix_moving)\n", + " ymin, ymax = axs[0].get_ylim()\n", + " plot_groundtruth_d(axs[0], color=\"white\")\n", + " plot_distance_estimates(axs[0], distance_estimates, label=\"chosen method\")\n", + " \n", + " plot_fg_estimates(axs[0], fg_dist_estimates, fg_colors)\n", + " axs[0].set_ylim(ymin, ymax)\n", + " \n", + " plot_groundtruth_d(axs[1], color=\"black\")\n", + " plot_distance_estimates(axs[1], distance_estimates, label=\"chosen method\")\n", + " plot_fg_estimates(axs[1], fg_dist_estimates, fg_colors)\n", + " axs[1].set_ylim(5, 100)\n", + " axs[1].grid(True)\n", + "\n", + " plot_std_estimates(axs[2], std_estimates, label=\"chosen method\")\n", + "\n", + " #plot_tail(axs[3], results_matrix_moving)\n", + " if not WallDetection.SIMPLIFY_ANGLES:\n", + " plot_angles(axs[3], results_matrix_angles)\n", + " return fig, axs" ] }, { @@ -1204,9 +1075,12 @@ "outputs": [], "source": [ "estimator = \"particle\"\n", + "appendix = \"test3\"\n", + "fname = f\"results/demo_results_matrices_{estimator}{appendix}.pkl\"\n", "try:\n", - " matrix_df = pd.read_pickle(f\"results/demo_results_matrices_{estimator}.pkl\")\n", + " matrix_df = pd.read_pickle(fname)\n", "except FileNotFoundError:\n", + " print(\"Could not find\", fname)\n", " print(\"Run generate_flying_results.py to generate results.\")\n", "matrix_df" ] @@ -1276,16 +1150,17 @@ " \n", "for estimator in [\"particle\", \"moving\"]:\n", " try:\n", - " matrix_df = pd.read_pickle(f\"results/demo_results_matrices_{estimator}.pkl\")\n", + " matrix_df = pd.read_pickle(f\"results/demo_results_matrices_{estimator}{appendix}.pkl\")\n", " except FileNotFoundError:\n", " print(\"Run generate_flying_results.py to generate results.\")\n", "\n", " n_windows = matrix_df[\"n window\"].unique()\n", + " print(n_windows)\n", " chosen_method = \"distance-mean\"\n", " chosen_dict = {\n", " \"n window\": None, # is filled below\n", " \"simplify angles\": False,\n", - " \"relative std\": 1.0,\n", + " \"relative std\": 0.0,\n", " \"calibration name\": \"iir\",\n", " \"calibration param\": 0.3,\n", " \"mask bad\": \"fixed\"\n", @@ -1300,7 +1175,9 @@ " chosen_dict[\"n window\"] = n_window\n", " rows = filter_by_dict(matrix_df, chosen_dict)\n", " #[print(key, matrix_df[key].unique()) for key in chosen_dict.keys()]\n", - " assert len(rows) == 1, len(rows)\n", + " if len(rows) != 1:\n", + " print(\"error:\", matrix_df, chosen_dict)\n", + " continue\n", " matrix = rows.iloc[0][\"matrix distances\"][0]\n", " precision, recall = get_precision_recall(matrix, distances_wall, method=chosen_method, verbose=False, sort=False) \n", "\n", @@ -1441,13 +1318,6 @@ ")" ] }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "### Sandbox" - ] - }, { "cell_type": "code", "execution_count": null, @@ -1534,10 +1404,10 @@ "\n", "n2_approx_pdf = n2_new.pdf(values) ** alpha\n", "n2_approx_pdf /= np.sum(n2_approx_pdf)\n", - "plt.plot(values, n2_approx_pdf, label=f'N1 ^ {alpha:.1f}', ls=\":\", color='C0')\n", + "plt.plot(values, n2_approx_pdf, label=f'$N_1 ^ {alpha:.1f}$', ls=\":\", color='C0')\n", "\n", "n2_conv = scipy.signal.fftconvolve(n1_pdf, n2_pdf, mode='same')\n", - "plt.plot(values, n2_conv, label=f'N1 * N2', ls=\":\", color='C1')\n", + "plt.plot(values, n2_conv, label=f'$N_1 N_2$', ls=\":\", color='C1')\n", "\n", "plt.legend()\n", "\n", @@ -1566,11 +1436,11 @@ " else:\n", " multimodal_approx_pdf[:int(len(multimodal) - shift)] = multimodal[shift:] ** alpha\n", " multimodal_approx_pdf /= np.sum(multimodal_approx_pdf)\n", - " plt.plot(values, multimodal_approx_pdf, label=f'P1 ^ {alpha:.1f} (std={scale:.1f})', ls=\"-\")\n", + " plt.plot(values, multimodal_approx_pdf, label=f'$P_1^{alpha:.1f}$ (std={scale:.1f})', ls=\"-\")\n", "\n", "multimodal_conv = scipy.signal.fftconvolve(multimodal, n2_pdf, mode='same')\n", "multimodal_conv /= np.sum(multimodal_conv)\n", - "plt.plot(values, multimodal_conv, label=f'P1 * N2', ls=\"-\")\n", + "plt.plot(values, multimodal_conv, label=f'$P_1 N_2$', ls=\"-\")\n", "plt.legend(loc='upper right')\n", "\n", "import timeit\n", @@ -1604,7 +1474,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.8.12" + "version": "3.8.10" }, "toc": { "base_numbering": 1, diff --git a/python/Makefile b/python/Makefile index 9edefde0..1977b8a6 100644 --- a/python/Makefile +++ b/python/Makefile @@ -1,29 +1,31 @@ # Generate pandas dataframes of results for faster processing. -EXPERIMENT_ROOT=../experiments +EXPERIMENT_ROOT=../datasets .PHONY: stepper_results -all: stepper_results epuck_results +all: crazyflie_demo crazyflie_flying crazyflie_stepper epuck_results crazyflie_demo: python csv_or_wav_parser.py --experiment_root $(EXPERIMENT_ROOT) --experiment_names 2021_11_23_demo --demo + python csv_or_wav_parser.py --experiment_root $(EXPERIMENT_ROOT) --experiment_names 2022_01_27_demo --demo + python generate_flying_results.py --experiment_root $(EXPERIMENT_ROOT) --experiment_names 2022_01_27_demo crazyflie_flying: python csv_or_wav_parser.py --experiment_root $(EXPERIMENT_ROOT) --experiment_names 2021_10_12_flying - python csv_or_wav_parser.py --experiment_root $(EXPERIMENT_ROOT) --experiment_names 2021_11_23_demo + python generate_flying_results.py --experiment_root $(EXPERIMENT_ROOT) --experiment_names 2021_10_12_flying -crazyflie_stepper_paper: - python csv_or_wav_parser.py --experiment_root $(EXPERIMENT_ROOT) --experiment_names 2021_10_07_stepper - python generate_stepper_results.py --experiment_root $(EXPERIMENT_ROOT) --experiment_names 2021_10_07_stepper - -crazyflie_stepper_thesis: +crazyflie_stepper: python csv_or_wav_parser.py --experiment_root $(EXPERIMENT_ROOT) --experiment_names 2021_07_08_stepper_fast python generate_stepper_results.py --experiment_root $(EXPERIMENT_ROOT) --experiment_names 2021_07_08_stepper_fast + python generate_flying_results.py --experiment_root $(EXPERIMENT_ROOT) --experiment_names 2021_07_08_stepper_fast + python generate_filtering_results.py --platform crazyflie epuck_results: python csv_or_wav_parser.py --experiment_root $(EXPERIMENT_ROOT) --experiment_names 2021_07_27_epuck_wall --platform epuck python generate_stepper_results.py --experiment_root $(EXPERIMENT_ROOT) --experiment_names 2021_07_27_epuck_wall --platform epuck + python generate_flying_results.py --experiment_root $(EXPERIMENT_ROOT) --experiment_names 2021_07_27_epuck_wall --platform epuck + python generate_filtering_results.py --platform epuck clean: rm $(EXPERIMENT_ROOT)/2021_07_08_stepper_fast/all_data.pkl diff --git a/python/StepperAnalysis.ipynb b/python/StepperAnalysis.ipynb index e12ad356..cddd6808 100644 --- a/python/StepperAnalysis.ipynb +++ b/python/StepperAnalysis.ipynb @@ -1,5 +1,16 @@ { "cells": [ + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# check that imports work\n", + "import rclpy\n", + "from audio_stack import beam_former" + ] + }, { "cell_type": "code", "execution_count": null, @@ -15,6 +26,9 @@ "import numpy as np\n", "import pandas as pd\n", "\n", + "import sys\n", + "sys.path.append(\"../src\")\n", + "\n", "%reload_ext autoreload\n", "%autoreload 2\n", "\n", @@ -47,6 +61,7 @@ "from utils.geometry import Context\n", "from utils.constants import PLATFORM\n", "\n", + "\n", "if PLATFORM == \"crazyflie\":\n", " from crazyflie_description_py.experiments import WALL_ANGLE_DEG_STEPPER\n", "else:\n", @@ -67,7 +82,7 @@ "[ax.scatter(*mic[:2], label=f'mic{i}, $\\\\theta=${azimuth_deg}$^\\\\circ$') for i, mic in enumerate(mic_positions)]\n", "\n", "source, mic_positions = get_setup(distance_cm=distance, azimuth_deg=azimuth_deg, ax=ax)\n", - "ax.set_title(f\"wall distance $d=${distance}cm \\n and angle $\\\\theta=${azimuth_deg}degrees\")\n", + "ax.set_title(f\"wall distance $d=${distance}cm \\n and angle $\\\\theta=${azimuth_deg} degrees\")\n", "handles, labels = ax.get_legend_handles_labels()\n", "h_mics = {l.split(', $\\\\theta')[0]: h for h, l in zip(handles, labels) if '\\\\theta' in l}\n", "l1 = ax.legend(h_mics.values(), h_mics.keys(),loc=\"lower left\", bbox_to_anchor=[0.6, 0])\n", @@ -90,11 +105,11 @@ " exp_name = \"2021_07_08_stepper_fast\"; # thesis\n", " #exp_name = \"2021_10_07_stepper\"\n", " motors = \"all45000\"\n", - " snr = 5\n", + " bin_selection = 5\n", "else:\n", " exp_name = \"2021_07_27_epuck_wall\"; # new sweep, better pipeline\n", " motors = \"sweep_and_move\" \n", - " snr = 0\n", + " bin_selection = 0\n", "mic_type = \"audio_deck\"" ] }, @@ -109,7 +124,7 @@ "from utils.data_collector import DataCollector\n", "\n", "data_collector = DataCollector()\n", - "backup_exists = data_collector.fill_from_backup(exp_name, mic_type, motors, snr)\n", + "backup_exists = data_collector.fill_from_backup(exp_name, mic_type, motors, bin_selection)\n", "if not backup_exists: \n", " print('generate data using script generate_df_results.')\n", "df_matrix, df_dist, df_freq = data_collector.get_df_matrix()" @@ -149,13 +164,13 @@ " fig, axs = plt.subplots(1, 2, sharey=True)\n", " fig.set_size_inches(10, 5)\n", " calib_function_median, freqs = get_calibration_function_median(\n", - " exp_name, mic_type, snr=snr, ax=axs[0], motors=motors_here\n", + " exp_name, mic_type, snr=bin_selection, ax=axs[0], motors=motors_here\n", " )\n", " axs[0].legend(loc='lower right')\n", " axs[0].set_title(\"calibration-individual\")\n", "\n", " calib_function_median_one, freqs = get_calibration_function_median(\n", - " exp_name, mic_type, ax=axs[1], fit_one_gain=True, snr=snr, motors=motors_here\n", + " exp_name, mic_type, ax=axs[1], fit_one_gain=True, snr=bin_selection, motors=motors_here\n", " )\n", " axs[1].set_title(\"calibration-global\")\n", " axs[1].set_ylabel('')\n", @@ -167,7 +182,7 @@ "cell_type": "code", "execution_count": null, "metadata": { - "scrolled": true + "scrolled": false }, "outputs": [], "source": [ @@ -190,13 +205,10 @@ "fig_all.set_size_inches(len(methods)*figsize, figsize) \n", "\n", "for j, method in enumerate(methods):\n", - " if method == \"raw\":\n", - " df_norm = df_matrix\n", - " elif method == \"theoretical\":\n", + " if method == \"theoretical\":\n", " df_norm = get_df_theory(df_freq, df_dist)\n", " elif method == \"measured\":\n", - " calib_values = calib_function_median(df_freq)[:, :, None]\n", - " df_norm = df_matrix #/ calib_values\n", + " df_norm = df_matrix\n", "\n", " n_mics = df_norm.shape[0]\n", " fig, axs = plt.subplots(1, n_mics, sharey=True)\n", @@ -216,9 +228,6 @@ " )\n", " ax.set_title(f\"mic{i} {method}\")\n", " ax.set_xlabel('distance [cm]')\n", - " #add_colorbar(fig, ax, im)\n", - " #save_fig(fig, f\"plots/{fname}_matrices_{normalization}.png\")\n", - " \n", " ax, im = plot_df_matrix(\n", " df_dist,\n", " df_freq,\n", @@ -236,6 +245,46 @@ "save_fig(fig_all, f\"{plot_dir}/{fname}_matrices_mic{mic_idx}.png\")" ] }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "from copy import deepcopy\n", + "from utils.inference import Inference\n", + "calib_function_median, freqs = get_calibration_function_median(\n", + " exp_name, mic_type, snr=bin_selection, ax=axs[0], motors=motors_here\n", + ")\n", + "\n", + "chosen_mics = [0,1,2,3]\n", + "d_estimates = {m:[] for m in chosen_mics}\n", + "plot_mic = 0\n", + "for i_d in range(df_matrix.shape[2]):\n", + " signals_f = deepcopy(df_matrix[chosen_mics, :, i_d]) # 4 x \n", + " signals_f /= calib_function_median(df_freq)[chosen_mics]\n", + " \n", + " inf_machine = Inference()\n", + " inf_machine.add_geometry(azimuth_deg=WALL_ANGLE_DEG_STEPPER, distance_range=[7, 100])\n", + " inf_machine.add_data(np.abs(signals_f), df_freq, mics=chosen_mics)\n", + " inf_machine.add_calibration_function(calib_function_median)\n", + " \n", + " #plt.figure()\n", + " for i in chosen_mics:\n", + " #dist_cm, probs, diff_cm = inf_machine.do_inference(algorithm=\"bayes\", mic_idx=i, calibrate=False, interpolate=True)\n", + " dist_cm, probs, diff_cm = inf_machine.do_inference(algorithm=\"cost\", mic_idx=i, calibrate=False, interpolate=False)\n", + " d_estimates[i].append(dist_cm[np.argmax(probs)])\n", + " #plt.plot(dist_cm, probs, color=f\"C{i}\", label=f\"mic{i}\")\n", + " #plt.legend()\n", + " #plt.title(f\"distance {i_d}\")\n", + " \n", + "fig, axs = plt.subplots(1, len(d_estimates.keys()), sharey=True)\n", + "for i, dist in d_estimates.items():\n", + " axs[i].scatter(range(len(dist)), dist, color=f\"C{i}\")\n", + " axs[i].set_title(f\"mic{i}\")\n", + " axs[i].grid()" + ] + }, { "cell_type": "markdown", "metadata": {}, @@ -249,12 +298,13 @@ "metadata": {}, "outputs": [], "source": [ + "import warnings\n", + "warnings.simplefilter(action='ignore', category=FutureWarning)\n", + "\n", "from plot_helpers import color, ls, units, labels\n", - "# current:\n", - "#plot_df = pd.read_pickle(\"results/stepper_results_timing.pkl\")\n", "\n", - "plot_df = pd.read_pickle(\"results/stepper_results_online_seed1.pkl\")\n", - "#plot_df = pd.read_pickle(\"results/stepper_results_online_seed1_uniform.pkl\")\n", + "plot_df = pd.read_pickle(\"results/stepper_results_online_seed1_uniform_new.pkl\")\n", + "\n", "plot_df = plot_df.loc[plot_df.method.str.startswith(\"calibrated\")]\n", "\n", "# add extra columns for plots\n", @@ -270,6 +320,7 @@ " \"histogram 1\": 0.05,\n", " \"histogram 3\": 0.1,\n", " \"histogram 5\": 0.15,\n", + " \"histogram\": 0\n", "}\n", "for (algorithm), df_dis in plot_df.groupby([\"algorithm\"]):\n", " fig, ax = plt.subplots()\n", @@ -289,7 +340,7 @@ " ls_name = method.split(color_name)[1].strip()\n", " \n", " label = labels[ls_name]\n", - " print(\"using linestyle\", ls[ls_name])\n", + " #print(\"using linestyle\", ls[ls_name])\n", "\n", " median = df.groupby(\"number angles\").median()\n", " median.error += shifts[ls_name]\n", @@ -301,12 +352,12 @@ " print(\"not plotting\", method)\n", " # sort particles by number\n", " particles_error = dict(sorted(particles_error.items()))\n", - " ax.plot(xs, particles_error.values(), ls=ls[\"particle\"], color='C0', label=\"particle\")\n", + " ax.plot(xs, list(particles_error.values()), ls=ls[\"particle\"], color='C0', label=\"particle\")\n", " particles_runtime = dict(sorted(particles_runtime.items()))\n", - " ax_time.plot(xs, particles_runtime.values(), ls=ls[\"particle\"], color='C1', label=\"particle\")\n", + " ax_time.plot(xs, list(particles_runtime.values()), ls=ls[\"particle\"], color='C1', label=\"particle\")\n", "\n", " xticklabels = [f\"{n_a:.0f}\\n{n_d:.0f}\\n{n_p:.0f}\" for (n_a, n_d, n_p) in zip(median.index, median[\"number distances\"], particles_error.keys())]\n", - " ax.set_xticks(xs, labels=xticklabels)\n", + " ax.set_xticks(xs, xticklabels)\n", "\n", " #leg = ax.legend(loc='upper left', bbox_to_anchor=[1.2, 1.0])\n", " #leg = ax.legend(loc='lower center',ncol=2,bbox_to_anchor=[0.5, 1.0])\n", @@ -320,7 +371,7 @@ " ax.yaxis.set_tick_params(colors=\"C0\")\n", " ax.set_title(\"time vs. performance\")\n", " #ax.set_title(f\"{algorithm}\")\n", - " save_fig(fig, f\"plots/experiments/{exp_name}_{algorithm.replace(' ', '_')}_time-error.pdf\")" + " #save_fig(fig, f\"plots/experiments/{exp_name}_{algorithm.replace(' ', '_')}_time-error.pdf\")" ] }, { @@ -328,17 +379,71 @@ "execution_count": null, "metadata": {}, "outputs": [], - "source": [] + "source": [ + "import warnings\n", + "warnings.simplefilter(action='ignore', category=FutureWarning)\n", + "\n", + "from plot_helpers import color, ls, units, labels\n", + "from generate_filtering_results import N_PARTICLES\n", + "\n", + "plot_df = pd.read_pickle(\"results/stepper_results.pkl\")\n", + "plot_df = plot_df.loc[plot_df.method.str.startswith(\"calibrated\")]\n", + "#print(plot_df)\n", + "\n", + "for (algorithm), df_dis in plot_df.groupby([\"algorithm\"]):\n", + " fig, ax = plt.subplots()\n", + " ax_time = ax.twinx()\n", + " particles_error = {}\n", + " particles_runtime = {}\n", + " \n", + " index_sorted = df_dis.discretization.unique()\n", + " pt = pd.pivot_table(df_dis, index=\"discretization\", values=[\"error\",\"runtime\"], aggfunc=lambda x: np.quantile(np.abs(x), 0.7), sort=False)\n", + " pt_std = pd.pivot_table(df_dis, index=\"discretization\", values=[\"error\",\"runtime\"], aggfunc=lambda x: 0.5*np.std(np.abs(x)), sort=False)\n", + " \n", + " x_ticks = [N_PARTICLES[dis] for dis in pt.index.values]\n", + " \n", + " ax.errorbar(x_ticks, pt.error.values, pt_std.error.values, color='C0')\n", + " ax_time.errorbar(x_ticks, pt.runtime.values, pt_std.runtime.values, color='C1')\n", + "\n", + " ax.grid(True)\n", + " ax.set_xlabel(\"\\# particles\")\n", + " ax.set_ylabel(f\"median error [{units[algorithm]}]\", color=\"C0\")\n", + " ax_time.set_ylabel(\"runtime [s]\", color=\"C1\")\n", + " ax_time.yaxis.set_tick_params(colors=\"C1\")\n", + " ax.yaxis.set_tick_params(colors=\"C0\")\n", + " ax.set_title(\"time vs. performance\")\n", + " #ax.set_title(f\"{algorithm}\")\n", + " #save_fig(fig, f\"plots/experiments/{exp_name}_{algorithm.replace(' ', '_')}_time-error.pdf\")" + ] }, { "cell_type": "code", "execution_count": null, - "metadata": {}, + "metadata": { + "scrolled": true + }, "outputs": [], "source": [ - "from plot_helpers import add_double_legend\n", - "#plot_df = pd.read_pickle(\"results/stepper_results_online_seed1_uniform.pkl\")\n", - "plot_df = pd.read_pickle(\"results/stepper_results_online_seed1.pkl\")\n", + "from plot_helpers import add_double_legend, labels, ls\n", + "\n", + "#plot_df = pd.read_pickle(\"results/stepper_results_online_seed1_uniform_new.pkl\")\n", + "#plot_df = pd.read_pickle(\"results/stepper_results_online_seed1.pkl\")\n", + "\n", + "beamform = False\n", + "\n", + "if PLATFORM == \"crazyflie\":\n", + " name = \"stepper\"\n", + "elif PLATFORM == \"epuck\":\n", + " name = \"epuck\"\n", + "print(name)\n", + "\n", + "if beamform:\n", + " fname = f\"results/{name}_results_beamform.pkl\"\n", + "else:\n", + " fname = f\"results/{name}_results.pkl\"\n", + "\n", + "plot_df = pd.read_pickle(fname)\n", + "print('read', fname)\n", "\n", "print(\"figsize\", figsize)\n", "X_MAX = {\n", @@ -348,12 +453,14 @@ "print(plot_df.discretization.unique())\n", "for discretization, df_ in plot_df.groupby(\"discretization\", sort=False):\n", " fig, axs = plt.subplots(1, 2)\n", - " fig.set_size_inches(4*figsize, 2*figsize)\n", + " fig.set_size_inches(8, 4)\n", " for i, (algorithm, df_dis) in enumerate(df_.groupby(\"algorithm\", sort=False)):\n", " for method, df in df_dis.groupby(\"method\"):\n", " color_name = method.split(' ')[0]\n", " ls_name = method.split(color_name)[1].strip()\n", - " if \"particle\" in ls_name:\n", + " if \"split particle\" in ls_name:\n", + " ls_name = \"split particle\"\n", + " elif \"particle\" in ls_name:\n", " ls_name = \"particle\"\n", "\n", " cdf = sorted(np.abs(df.error))\n", @@ -372,303 +479,58 @@ " axs[i].yaxis.set_label_coords(-0.15, 0.5)\n", " add_double_legend(axs[i])\n", " fig.subplots_adjust(wspace=0.3)\n", - " save_fig(fig, f\"plots/experiments/{exp_name}_{discretization.replace(' ', '_')}_cdfs.pdf\")" + " if beamform: \n", + " save_fig(fig, f\"plots/experiments/{exp_name}_{discretization.replace(' ', '_')}_cdfs_beamform.pdf\")\n", + " else:\n", + " save_fig(fig, f\"plots/experiments/{exp_name}_{discretization.replace(' ', '_')}_cdfs.pdf\")" ] }, { "cell_type": "code", "execution_count": null, - "metadata": { - "scrolled": false - }, + "metadata": {}, "outputs": [], "source": [ - "from utils.pandas_utils import filter_by_dict\n", + "fname = f\"results/stepper_results.pkl\"\n", "\n", - "#plot_df = pd.read_pickle(\"results/stepper_results_online_seed1_uniform.pkl\")\n", - "plot_df = pd.read_pickle(\"results/stepper_results_online_seed1.pkl\")\n", + "plot_df = pd.read_pickle(fname)\n", + "print('read', fname)\n", "\n", - "#chosen_dicts = {\n", - " #\"algorithm\": [\"bayes distance\", \"bayes angle\"],\n", - " #\"discretization\": \"medium\"\n", - "#}\n", - "#plot_df = filter_by_dict(plot_df, chosen_dict)\n", - "#chosen_dict = {}\n", + "X_MAX = {\n", + " \"bayes angle\": 200,\n", + " \"bayes distance\": 30\n", + "}\n", "\n", - "for discretization, df_ in plot_df.groupby(\"discretization\", sort=False):\n", - " fig_d, ax_d = plt.subplots()\n", - " fig_d.set_size_inches(1.7*figsize, 2*figsize)\n", - " fig_a, ax_a = plt.subplots()\n", - " fig_a.set_size_inches(1.7*figsize, 2*figsize)\n", - " axs_da = [ax_d, ax_a]\n", - " \n", - " fig, axs = plt.subplots(1, 2)\n", - " fig.set_size_inches(4*figsize, 2*figsize)\n", + "linestyles = {\n", + " \"calibrated\": \"-\",\n", + " \"theoretical\": \":\",\n", + " \"fixed\":\"\",\n", + " \"random\":\"\",\n", + "}\n", + "\n", + "fig, axs = plt.subplots(1, 2, sharey=True)\n", + "fig.set_size_inches(8, 4)\n", + "cmap = plt.get_cmap(\"viridis\", lut=len(plot_df.discretization.unique()))\n", + "for j, (discretization, df_) in enumerate(plot_df.groupby(\"discretization\", sort=False)):\n", " for i, (algorithm, df_dis) in enumerate(df_.groupby(\"algorithm\", sort=False)):\n", - " ax = axs_da[i]\n", " for method, df in df_dis.groupby(\"method\"):\n", - " color_name = method.split(' ')[0]\n", - " if color_name in [\"fixed\", \"random\"]:\n", - " continue \n", - " if \"particle\" in method:\n", - " ls_name = \"particle\"\n", - " else:\n", - " ls_name = method.split(color_name)[1].strip()\n", - "\n", + " if method in [\"fixed\", \"random\"]:\n", + " continue\n", + " ls_name, p, n = method.split(' ')\n", " cdf = sorted(np.abs(df.error))\n", - " if color_name == \"calibrated\":\n", - " label = labels[ls_name]\n", - " else:\n", - " label = None\n", - " \n", - " for ax in [axs[i], ax]:\n", - " ax.plot(df.distance, df.error, color=color[color_name], linestyle=ls[ls_name], label=label)\n", - " ax.grid(True)\n", - " ax.set_title(f\"{algorithm.replace('bayes ','')} estimation \")\n", - " ax.set_xlabel(\"real distance [cm]\")\n", - " ax.set_ylabel(f\"error [{units[algorithm]}]\", y=0.5, x=0.01)\n", - " ax.yaxis.set_label_coords(-0.15, 0.5)\n", - " #add_double_legend(ax, exclude=[\"fixed\", \"random\"])\n", - " \n", - " add_double_legend(axs[i], exclude=[\"fixed\", \"random\"])\n", - " fig.subplots_adjust(wspace=0.3)\n", - " save_fig(fig, f\"plots/experiments/{exp_name}_{discretization.replace(' ', '_')}_errors.pdf\")\n", - " \n", - " fig_d.subplots_adjust(wspace=0.3)\n", - " save_fig(fig_d, f\"plots/experiments/{exp_name}_{discretization.replace(' ', '_')}_errors_d.pdf\")\n", - " fig_a.subplots_adjust(wspace=0.3)\n", - " save_fig(fig_a, f\"plots/experiments/{exp_name}_{discretization.replace(' ', '_')}_errors_a.pdf\")" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "scrolled": false - }, - "outputs": [], - "source": [ - "# use the different online schemes and compare\n", - "df_mat = pd.read_pickle(\"results/stepper_results_matrices.pkl\")\n", - "df_mat = df_mat[(df_mat[\"relative std\"] == 1.0) \n", - " & (df_mat[\"mask bad\"]==\"fixed\")]\n", - "\n", - "for (calibration, n_window), df_calib in df_mat.groupby([\"calibration name\", \"n window\"]):\n", - " fig, axs = plt.subplots(1, 2)\n", - " fig.set_size_inches(10, 5)\n", - " for i, row in df_calib.iterrows():\n", - " \n", - " label = f'{round(row[\"calibration param\"], 1)}'\n", - " matrix = row[\"matrix distances\"][0]\n", - " distances_cm = row[\"distances_cm\"]\n", - " \n", - " real_distances = np.arange(matrix.shape[1]) + 7\n", - " distance_estimates = distances_cm[np.argmax(matrix, axis=0)]\n", - " axs[0].plot(np.arange(matrix.shape[1]), real_distances - distance_estimates, label=label)\n", - " \n", - " cdf = np.sort(np.abs(real_distances - distance_estimates))\n", - " axs[1].plot(cdf, np.linspace(0, 1, len(cdf)), label=label)\n", - " axs[0].grid(True)\n", - " axs[1].grid(True)\n", - " axs[1].set_xlim(0, 50)\n", - " axs[0].set_ylim(-50, 50)\n", - " axs[1].legend(loc=\"upper left\", bbox_to_anchor=[1.0, 1.0])\n", - " #plt.plot(np.arange(matrix.shape[1]), real_distances, color='k')\n", - " plt.ylabel(\"error [cm]\")\n", - " #plt.suptitle(f\"$N_w$={n_window}, calibration {calibration}\", y=0.95)\n", - " plt.suptitle(f\"calibration: {calibration}\", y=0.95)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "## TODO(FD): fix below if necessary. \n", - "\n", - "from utils.inference import eps_normalize\n", - "from utils.simulation import get_freq_slice_theory\n", - "from utils.plotting_tools import all_linestyles\n", - "\n", - "mic_idx = 1\n", - "eps = 1e-5\n", - "\n", - "plot_combis = [\n", - " #{\"distance\": 15, \"algorithm\": \"cost\"},\n", - " {\"distance\": 15, \"algorithm\": \"bayes\"},\n", - " #{\"distance\": 17, \"algorithm\": \"bayes\"},\n", - " #{\"distance\": 28, \"algorithm\": \"bayes\"},\n", - "]\n", - "print(\"using uniform prior:\", use_uniform_prior)\n", - "\n", - "for plot_combi in plot_combis:\n", - " distance = plot_combi.get(\"distance\")\n", - " algorithm = plot_combi.get(\"algorithm\")\n", - "\n", - " shape = (1, n_mics)\n", - " # plot the slices\n", - " fig_slice, axs_slice = plt.subplots(*shape, sharex=True, sharey=True)\n", - " fig_slice.set_size_inches(3 * shape[1], 2 * shape[0])\n", - " # plot the results \n", - " fig_algo, axs_algo = plt.subplots(*shape, sharex=True, sharey=True)\n", - " fig_algo.set_size_inches(3 * shape[1], 2 * shape[0])\n", - "\n", - " distance_estimators = {k: DistanceEstimator() for k in method_dict.keys()}\n", - " for i_method, (method, normalize_method) in enumerate(method_dict.items()):\n", - " if method == \"theoretical\":\n", - " slice_exp = get_freq_slice_theory(df_freq,\n", - " distance,\n", - " azimuth_deg=azimuth_deg, \n", - " chosen_mics=chosen_mics)\n", - " slice_exp = slice_exp.T # now it's 4x32\n", - " freqs = df_freq\n", - " else:\n", - " slice_exp, freqs, stds = data_collector.get_frequency_slice_fixed(\n", - " df_freq,\n", - " distance,\n", - " mics=chosen_mics)\n", - " slice_exp = np.sqrt(slice_exp)\n", - " \n", - " \n", - " inf_machine.add_data(slice_exp, df_freq, mics=chosen_mics)\n", - " inf_machine.filter_out_freqs()\n", - " inf_machine.add_calibration_function(normalize_method)\n", - "\n", - " for i_mic, mic_idx in enumerate(chosen_mics):\n", - " distances_bayes, proba_bayes_norm, diff_bayes = inf_machine.do_inference(\n", - " algorithm=\"bayes\", \n", - " mic_idx=i_mic,\n", - " #normalize=False\n", - " )\n", - " \n", - " inf_machine.plot(i_mic=i_mic, ax=axs_slice[i_mic], label=method, standardize=True,\n", - " ls=all_linestyles[i_method])#marker='o')\n", - "\n", - " distance_estimators[method].add_distribution(\n", - " diff_bayes * 1e-2, proba_bayes_norm, mic_idx\n", - " )\n", - " d = get_estimate(distances_bayes, proba_bayes_norm)\n", - "\n", - " axs_algo[i_mic].semilogy(\n", - " distances_bayes,\n", - " eps_normalize(proba_bayes_norm, eps=eps),\n", - " color=f\"C{i_method}\",\n", - " ls=all_linestyles[i_method],\n", - " label=method,\n", - " )\n", - " axs_algo[i_mic].axvline(x=d, color=f\"C{i_method}\", ls=\":\") \n", - " axs_algo[i_mic].set_title(f\"mic{mic_idx}\")\n", - " axs_slice[i_mic].set_title(f\"mic{mic_idx}\")\n", + " #axs[i].plot(cdf, np.linspace(0, 1, len(cdf)), color=color[color_name], linestyle=ls[ls_name], label=label)\n", + " label = f\"$N_p$={n}\" if ls_name==\"calibrated\" else None\n", + " axs[i].plot(cdf, np.linspace(0, 1, len(cdf)), color=cmap(j), linestyle=linestyles[ls_name], label=label)\n", " \n", - " label_real = f\"real: {distance:.0f}cm\"\n", - " #axs_slice[-1].legend(title=label_real, bbox_to_anchor=[1.0, 1.0], loc='upper left')\n", - " #axs_algo[-1].legend(title=label_real, bbox_to_anchor=[1.0, 1.0], loc='upper left')\n", - " axs_slice[0].set_ylabel('amplitude') \n", - " axs_algo[0].set_ylabel('probability') \n", - " [ax.set_xlabel('frequency [Hz]') for ax in axs_slice]\n", - " [ax.set_xlabel('distance [cm]') for ax in axs_algo]\n", - "\n", - " fname_here = f\"{plot_dir}/{fname}_{distance}cm_raw_slices.png\"\n", - " save_fig(fig_slice, fname_here)\n", - " fname_here = f\"{plot_dir}/{fname}_{distance}cm_raw_results.png\"\n", - " save_fig(fig_algo, fname_here)\n", - "\n", - " fig_combi, axs_combi = plt.subplots(1, 2)\n", - " fig_combi.set_size_inches(6, 3)\n", - " \n", - " print(\"getting distance distribution...\")\n", - " ax_combi = axs_combi[0]\n", - " for i_method, (method, distance_estimator) in enumerate(\n", - " distance_estimators.items()\n", - " ):\n", - " azimuth_deg_here = None if use_uniform_prior else azimuth_deg_here\n", - " distances_m, proba = distance_estimator.get_distance_distribution(\n", - " azimuth_deg=azimuth_deg_here, distances_m=distances_grid_m, verbose=False,\n", - " method=\"sum\"\n", - " )\n", - " print(distances_m, proba)\n", - " ax_combi.plot(\n", - " distances_m*1e2,\n", - " eps_normalize(proba, eps=eps),\n", - " color=f\"C{i_method}\",\n", - " ls=all_linestyles[i_method],\n", - " label=method,\n", - " )\n", - " ax_combi.axvline(x=distance, color=\"black\", ls=\":\")\n", - " ax_combi.set_xlabel(\"distance [cm]\")\n", - " ax_combi.set_ylabel(\"probability\")\n", - " #ax_combi.set_title(\"total probability\")\n", - " ax_combi.set_yscale(\"log\")\n", - "\n", - " print(\"getting angle distribution...\")\n", - " ax_combi = axs_combi[1]\n", - " for i_method, (method, distance_estimator) in enumerate(\n", - " distance_estimators.items()\n", - " ):\n", - "\n", - " # doesn't work with uniform prior!!\n", - " distance_m_here = d * 1e-2\n", - " angles, proba = distance_estimator.get_angle_distribution(\n", - " distance_estimate_m=distance_m_here,\n", - " azimuths_deg=np.arange(360),\n", - " method=\"sum\",\n", - " )\n", - " ax_combi.plot(\n", - " angles,\n", - " eps_normalize(proba, eps=eps),\n", - " color=f\"C{i_method}\",\n", - " ls=all_linestyles[i_method],\n", - " label=method,\n", - " )\n", - " ax_combi.axvline(\n", - " x=azimuth_deg, color=\"black\", ls=\":\", label=f\"real: {distance:.0f}cm, {azimuth_deg:.0f}$^\\\\circ$\"\n", - " )\n", - " ax_combi.legend(loc=\"upper left\", bbox_to_anchor=[1.0, 1.0])\n", - " ax_combi.set_xlabel(\"angle [$^\\\\circ$]\")\n", - " #ax_combi.set_title(\"total probability\")\n", - " ax_combi.set_yscale(\"log\")\n", - " plt.subplots_adjust(wspace=0.25)\n", - "\n", - " fname_here = f\"{plot_dir}/{fname}_{distance}cm_raw_total.png\"\n", - " save_fig(fig_combi, fname_here)" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "# 1. Distance slices" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "scrolled": false - }, - "outputs": [], - "source": [ - "def plot_slices(slice_exp, slice_the, dist):\n", - " fig_f, axs_f = plt.subplots(1, slice_exp.shape[0], squeeze=False, sharey=True)\n", - " fig_f.set_size_inches(2*FIGSIZE, FIGSIZE)\n", - " fig_f.suptitle(f\"Standardized distance slices, at frequency {f:.0f}Hz\")\n", - " for m in range(slice_exp.shape[0]):\n", - " slice_exp_norm = deepcopy(slice_exp[m])\n", - " slice_the_norm = deepcopy(slice_the[m])\n", - " slice_exp_norm -= np.mean(slice_exp_norm)\n", - " slice_the_norm -= np.mean(slice_the_norm)\n", - " slice_exp_norm /= np.std(slice_exp_norm)\n", - " slice_the_norm /= np.std(slice_the_norm)\n", - " \n", - " axs_f[0, m].plot(dist, slice_the_norm, label=\"theoretical\")\n", - " axs_f[0, m].plot(dist, slice_exp_norm, label=\"measured\")\n", - " axs_f[0, m].set_title(f\"mic{m}\")\n", - " axs_f[0, m].legend(loc=\"upper right\")\n", - " axs_f[0, m].set_xlabel(\"distance [cm]\")\n", - " axs_f[0, m].grid()\n", - " axs_f[0, 0].set_ylabel(\"amplitude\")\n", - " return fig_f" + " axs[i].grid(True)\n", + " axs[i].set_xlim(0, X_MAX[algorithm])\n", + " #axs[i].set_title(f\"{discretization} discretization, {algorithm}\")\n", + " axs[i].set_title(f\"{algorithm.replace('bayes', '')} estimation\")\n", + " axs[i].set_xlabel(f\"absolute error [{units[algorithm]}]\")\n", + "axs[1].legend()\n", + "axs[0].set_ylabel(\"cdf [-]\")\n", + "#axs[i].yaxis.set_label_coords(-0.15, 0.5)\n", + "save_fig(fig, f\"plots/experiments/{exp_name}_complexity_cdfs.pdf\")" ] }, { @@ -677,25 +539,14 @@ "metadata": {}, "outputs": [], "source": [ - "from utils.pandas_utils import fill_nans\n", - "from utils.simulation import get_dist_slice_theory\n", - "from copy import deepcopy\n", - "\n", - "for i, f in enumerate(df_freq):\n", - " \n", - " slice_exp = df_matrix[:, i, :]\n", - " if np.any(np.isnan(slice_exp)):\n", - " slice_exp = fill_nans(slice_exp, df_dist)\n", - " slice_the = get_dist_slice_theory(f, df_dist, azimuth_deg=azimuth_deg).T\n", - " \n", - " fig_d = plot_slices(\n", - " slice_exp=slice_exp,\n", - " slice_the=slice_the,\n", - " dist=df_dist,\n", - " )\n", - " fname = f\"{exp_name}_{motors}\"\n", - " if f in [2562, 3375, 4500]: # manually picked these\n", - " save_fig(fig_d, f'{plot_dir}/{fname}_distance_slice_{f:.0f}.pdf')" + "from utils.constants import SPEED_OF_SOUND\n", + "print(freqs)\n", + "delta_f_med = np.median(np.diff(freqs))\n", + "max_d_med = SPEED_OF_SOUND / (4 * delta_f_med)\n", + "delta_f_min = np.min(np.diff(freqs))\n", + "max_d_min = SPEED_OF_SOUND / (4 * delta_f_min)\n", + "print(\"median:\", max_d_med)\n", + "print(\"minimum:\", max_d_min)" ] }, { @@ -704,239 +555,59 @@ "metadata": {}, "outputs": [], "source": [ - "from utils.simulation import factor_distance_to_delta\n", - "\n", - "d1 = df_dist[-1] # starting distance of distance slice. \n", - "rel_movement_cm = df_dist[1]-df_dist[0]\n", - "factors_max = {mic:factor_distance_to_delta(d1, \n", - " rel_movement_cm, \n", - " mic, azimuth_deg=azimuth_deg) \n", - " for mic in range(4)}\n", - "print('factors_max:\\n', factors_max)\n", - "dN = df_dist[0] # ending distance of distance slice. \n", - "factors_min = {mic:factor_distance_to_delta(dN, \n", - " rel_movement_cm, \n", - " mic, azimuth_deg=azimuth_deg) \n", - " for mic in range(4)}\n", - "print('factors_min:\\n', factors_min)\n", - "print('average', np.mean(list(factors_max.values())+list(factors_min.values())))" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "scrolled": false - }, - "outputs": [], - "source": [ - "from utils.inference import get_approach_angle_fft, get_approach_angle_cost\n", - "from utils.plotting_tools import plot_performance, save_fig, titles, linestyles\n", - "from utils.estimators import get_estimate, AngleEstimator\n", - "from utils.inference import BAD_FREQ_RANGES, PLATFORM\n", - "\n", - "plot_slices = True\n", - "gamma_min = 10\n", - "\n", - "start_distances_grid = [max(df_dist)+10] #[max(df_dist) + i for i in [0, 10, 20]]\n", - "gammas_grid = np.arange(gamma_min, 91, step=1)\n", - "\n", - "factor = 2.0 # correction factor, 2 means we assume colocated\n", - "gt_gamma = 90 # in degrees\n", - "\n", - "fig, ax_total = plt.subplots()\n", - "ax_total.set_xlabel('approach angle $\\\\gamma$')\n", - "ax_total.set_ylabel('probability')\n", - "ax_total.set_title('combined distribution across mics and frequencies')\n", - "\n", - "#for algo in [\"bayes\"]#[\"cost\", \"bayes\"]:\n", - "#algo = \"cost\" #\"bayes\"\n", - "algo = \"bayes\" #\"bayes\"\n", - " \n", - "estimator_all = AngleEstimator()\n", - "\n", - "labels = [\"all mics\"] + [f\"mic{m}\" for m in range(df_matrix.shape[0])] \n", - "err_dict = {l: [np.nan]*len(df_freq) for l in labels}\n", - "\n", - "print(df_freq)\n", - "for i, f in enumerate(df_freq):\n", - " if np.any([r[0] <= f <= r[1] for r in BAD_FREQ_RANGES]):\n", - " print('skipping', f)\n", - " continue\n", - " estimator = AngleEstimator()\n", - "\n", - " d_slices = fill_nans(df_matrix[:, i, :], df_dist)\n", - "\n", - " if plot_slices:\n", - " fig, ax = plt.subplots()\n", - " fig.set_size_inches(7, 5)\n", - " for mic_idx in range(df_matrix.shape[0]):\n", - " d_slice = d_slices[mic_idx] ** 2\n", - "\n", - " if algo == \"bayes\":\n", - " gammas, prob = get_approach_angle_fft(d_slice, f, df_dist,\n", - " n_max=1000, bayes=True, \n", - " reduced=False, \n", - " factor=factor)\n", - " elif algo == \"cost\":\n", - " prob = get_approach_angle_cost(\n", - " d_slice,\n", - " f,\n", - " df_dist,\n", - " start_distances_grid,\n", - " gammas_grid,\n", - " mic_idx=mic_idx,\n", - " azimuth_deg=azimuth_deg\n", - " ) # is of shape n_start_distances x n_gammas_grid\n", - " gammas = gammas_grid\n", - " \n", - " prob = prob[gammas > gamma_min]\n", - " gammas = gammas[gammas > gamma_min]\n", - " \n", - " estimator_all.add_distribution(gammas, prob, mic_idx, f)\n", - " estimator.add_distribution(gammas, prob, mic_idx, f)\n", - "\n", - " gamma = get_estimate(gammas, prob)\n", - " \n", - " err_dict[f\"mic{mic_idx}\"][i] = gamma - gt_gamma\n", - "\n", - " if plot_slices:\n", - " ax.plot(gammas, prob)\n", - " ax.axvline(gamma, label=f'mic{mic_idx}, {algo}', color=f'C{mic_idx}', ls=linestyles[algo])\n", - "\n", - " if plot_slices:\n", - " ax.set_title(f'frequency {f:.0f}Hz')\n", - " ax.set_xlabel('angle of approach $\\\\gamma$ [$^\\\\circ$]')\n", - " ax.set_ylabel('probability')\n", - " ax.legend()\n", - "\n", - " #angles, prob = estimator.get_angle_distribution(mics_left_right=[[1, 2], [0, 3]])\n", - " angles, prob = estimator.get_angle_distribution(mics_left_right=None)\n", - " \n", - " gamma = get_estimate(angles, prob)\n", - " err_dict[\"all mics\"][i] = gamma - gt_gamma\n", - "\n", - "#angles, prob = estimator_all.get_angle_distribution(mics_left_right=[[1, 2], [0, 3]])\n", - "angles, prob = estimator_all.get_angle_distribution(mics_left_right=None)\n", - "ax_total.plot(angles, prob, label=algo)\n", - "\n", - "fname = f\"{exp_name}_{motors}\"\n", - "fig, axs = plot_performance(err_dict, xs=df_freq, \n", - " xlabel=\"frequency [Hz]\", ylabel=\"error [$^\\\\circ$]\")\n", - "max_err = 90 #45\n", - "axs[1].set_xlim(0, max_err)\n", - "axs[0].set_ylim(-max_err, max_err)\n", - "save_fig(fig, f'{plot_dir}/{fname}_{algo}_distance_slice_performance.pdf')\n", - "\n", - "ax_total.legend()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "# 4. Calibration analysis" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "exp_name = \"2021_07_08_stepper_fast\"\n", - "mic_type = \"audio_deck\"\n", - "motors = \"all45000\"\n", - "snr = 5\n", - "plot_tuples = [(0, 3125)]\n", + "from plot_helpers import add_double_legend\n", + "from utils.pandas_utils import filter_by_dict\n", "\n", - "data_collector = DataCollector()\n", - "backup_exists = data_collector.fill_from_backup(exp_name, mic_type, motors, snr=snr)\n", - "if not backup_exists: \n", - " print('generate data using script generate_df_results.')" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "from utils.calibration import fit_distance_slice\n", - "from utils.simulation import get_amplitude_function\n", + "def convert_to_str(row):\n", + " chosen_mic = row.chosen_mics\n", + " chosen_mic_str = \"-\".join(map(str, chosen_mic))\n", + " row.chosen_mics = chosen_mic_str\n", + " return row\n", "\n", - "fname = f'{exp_name}_{motors}'\n", - "all_distances = data_collector.df.distance.unique()\n", + "plot_df = pd.read_pickle(f\"results/{name}_results_mics_ablation.pkl\")\n", "\n", - "mic_idx = 0\n", - "max_distance = 35\n", + "X_MAX = {\n", + " \"bayes angle\": plot_df[plot_df.algorithm==\"bayes angle\"].error.max(),\n", + " \"bayes distance\": 30 #plot_df[plot_df.algorithm==\"bayes distance\"].error.max()\n", + "}\n", "\n", - "for i, (frequency, df_freq_here) in enumerate(data_collector.df.groupby('frequency')):\n", - " distances = df_freq_here.distance.unique()\n", - " if len(distances) < len(all_distances): # only plot distances with \"full coverage\"\n", - " continue\n", + "plot_df = plot_df.loc[~plot_df.method.isin([\"fixed\", \"random\"])]\n", + "plot_df = plot_df.apply(convert_to_str, axis=1)\n", + "discretization = plot_df.discretization.unique()[0]\n", + "\n", + "colors = plt.get_cmap(\"viridis\", lut=len(plot_df.chosen_mics.unique()))\n", + "\n", + "fig, axs = plt.subplots(1, 2, sharey=True)\n", + "fig.set_size_inches(6, 2)\n", + "\n", + "for i, (algorithm, df_dis) in enumerate(plot_df.groupby(\"algorithm\", sort=False)):\n", + " boxplots = {}\n", + " for j, (chosen_mics, df) in enumerate(df_dis.groupby(\"chosen_mics\", sort=False)):\n", + " #just a sanity check that each distance only appears once\n", + " if not len(df.distance) == len(df.distance.unique()):\n", + " print(df)\n", + " break\n", + " boxplots[chosen_mics] = df.error\n", + " cdf = sorted(np.abs(df.error))\n", + " axs[i].plot(cdf, np.linspace(0, 1, len(cdf)), label=chosen_mics, color=colors(j))\n", + " axs[i].grid(True)\n", + " axs[i].set_xlim(0, X_MAX[algorithm])\n", + " axs[i].set_title(f\"{algorithm.replace('bayes', '')} estimation\")\n", + " axs[i].set_xlabel(f\"absolute error [{units[algorithm]}]\")\n", + "axs[0].set_ylabel(\"cdf [-]\")\n", " \n", - " df_here = df_freq_here.loc[df_freq_here.mic==mic_idx]\n", - " \n", - " coeffs_median, distances_median, fit_median, cost_median = data_collector.fit_to_median(frequency, [mic_idx])\n", - " alpha, offset, gain = coeffs_median\n", - " \n", - " # find the sigma for this frequency (per distance)\n", - " std_series = df_here.groupby('distance').magnitude.std() # per distance\n", - " std_average = np.nanmean(std_series.values)\n", - " amps = get_amplitude_function(std_series.index, \n", - " gain, \n", - " alpha, mic_idx)\n", - " valid_distances = std_series.index[amps >= std_average]\n", - " limit_distance = valid_distances[-1] if len(valid_distances) else 0\n", + "#fig_box, ax_plot = plt.subplots()\n", + "#ax_plot.boxplot(boxplots.values())\n", + "#ax_plot.set_xticklabels(boxplots.keys(), rotation=45)\n", "\n", - " #print(f'raw: mic {mic}, frequency {frequency}')\n", - " #print(f'median: alpha={alpha:.2f}, gain={gain:.2f}, offset={offset:.0f}, cost={cost_median}')\n", "\n", - "\n", - " label = f\"{frequency:.0f}Hz\"\n", - " fig, axs = plt.subplots(1, 2)\n", - " fig.set_size_inches(10, 5)\n", - " ax_fit, ax_freq = axs\n", - " \n", - " slices_median, distances_median_all, mics, stds = data_collector.get_distance_slice(frequency)\n", - " for d, series in df_here.groupby('distance').magnitude: \n", - " ax_fit.scatter([d]*len(series), series.values, color='C0', s=4)#, s=5.0)\n", - " ax_fit.scatter([], [], color='C0', label='raw', s=4)#, s=5.0)\n", - "\n", - " ax_fit.plot(distances_median_all, slices_median[mic_idx, :], color='C0', label='distance slice')\n", - " ax_fit.plot(distances_median, fit_median, color='C1', label='model fit')\n", - "\n", - " ax_fit.set_title('fit for ' + label)\n", - " ax_fit.set_ylabel('amplitude [-]')\n", - " ax_fit.set_xlabel('distance [cm]')\n", - " ax_fit.legend(loc='lower left')\n", - " ax_fit.set_xlim(min(distances_median), max_distance)\n", - "\n", - " ax_freq.semilogy(std_series.index, amps, ls=':', color=f'C0', label=f'magnitude')\n", - " ax_freq.axhline(std_average, label=f'average std', color=f'C0')\n", - " ax_freq.scatter(std_series.index, std_series.values, label=f'std', color=f'C0', s=4)\n", - " ax_freq.set_xlim(min(distances_median), max_distance)\n", - "\n", - " ax_freq.set_title('magnitude vs. noise for ' + label)\n", - " ax_freq.legend(loc='lower left')\n", - " ax_freq.set_xlabel('distance [cm]')\n", - " #ax_freq.set_ylabel('magnitude vs. std [-]')\n", - "\n", - " if (mic_idx, frequency) not in plot_tuples:\n", - " continue\n", - " \n", - " fname_here = f'{plot_dir}/{fname}_fitting_{frequency:.0f}_{mic_idx}.png'\n", - " save_fig(fig, fname_here)" + "fig.subplots_adjust(wspace=0.1)\n", + "axs[1].legend(loc=\"upper left\", bbox_to_anchor=[1.0,1.0], title=\"used mics\", ncol=2)\n", + "handles, labels = axs[i].get_legend_handles_labels()\n", + "axs[1].legend(handles[::1], labels[::1],ncol=2,fontsize=10, title=\"used mics\", loc=\"lower left\", bbox_to_anchor=[1.0, -0.2])\n", + "save_fig(fig, f\"plots/experiments/{exp_name}_{discretization.replace(' ', '_')}_mics_cdfs.pdf\")" ] }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [] - }, { "cell_type": "code", "execution_count": null, @@ -961,7 +632,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.8.12" + "version": "3.8.10" }, "toc": { "base_numbering": 1, diff --git a/python/WallStudy.ipynb b/python/WallStudy.ipynb index ecfc7960..78a25d54 100644 --- a/python/WallStudy.ipynb +++ b/python/WallStudy.ipynb @@ -29,13 +29,116 @@ " \"figure.figsize\": (10, 5),\n", " \"figure.max_open_warning\": False,\n", " \"text.usetex\": True,\n", - " \"font.family\": \"sans-serif\",\n", - " \"font.sans-serif\": [\"Helvetica\"],\n", + " \"font.family\": \"DejaVu Sans\",\n", " }\n", ")\n", "figsize = 3" ] }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# FCMW Study\n", + "\n", + "c = 343 # m / s\n", + "distances_cm = np.arange(30, step=3)\n", + "times = distances_cm * 1e-2 / c\n", + "sampling_rates = 16000 * np.arange(1, 5)\n", + "\n", + "N = 2**np.arange(4, 15)\n", + "\n", + "fig = plt.figure()\n", + "fig.set_size_inches(5, 5)\n", + "for FS in sampling_rates:\n", + " f_smears = []\n", + " for n in N:\n", + " f_smears.append(n / FS)\n", + " plt.plot(f_smears, label = f\"N / {FS}Hz\", ls=\":\")\n", + " \n", + "cmap = plt.get_cmap('viridis', lut=len(times))\n", + "for i, t in enumerate(times):\n", + " plt.axhline(t, color=cmap(i), label=f\"{distances_cm[i]:.0f}cm\")\n", + " \n", + "plt.ylim(0, max(times))\n", + "plt.ylabel(\"time $t$ [s]\")\n", + "plt.grid()\n", + "\n", + "plt.xticks(range(len(N)), N)\n", + "plt.xlabel(\"N\")\n", + "plt.legend()\n", + "plt.title(\"regions where $t$ is bigger than N/FS are valid.\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "chosen_N = 32\n", + "chosen_d_cm = 20\n", + "chosen_FS = 64000\n", + "\n", + "factor = chosen_d_cm * 1e-2 / c - chosen_N / chosen_FS\n", + "fres = chosen_FS / chosen_N\n", + "\n", + "print(\"Comparison of bandwidth / pulse length requirements:\")\n", + "print(\"F/T needs to be much bugger than\", fres / factor)\n", + "T_chosen = 1 #1e-3\n", + "print(f\"for T={T_chosen} s, F needs to be: {fres / factor * T_chosen:.0f} Hz\", )\n", + "F_chosen = 3000\n", + "print(f\"for F={F_chosen} Hz, T needs to be: {F_chosen / (fres / factor) * 1e3:.3f} ms\")\n", + "\n", + "# need F / T such that F/T * " + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "chosen_t = chosen_d_cm * 1e-2 / c\n", + "N_admissible = chosen_t * sampling_rates\n", + "print(\"admissible Ns: smaller than\", N_admissible)\n", + "print(\"addmissible Ns: bigger than:\", sampling_rates / chosen_t)\n", + "k_min = 1 / chosen_t**2\n", + "print(\"minimum value for k = F/T >=\", k_min)\n", + "\n", + "print(f\"if F is 3000 Hz, T <= {3000/k_min*1e3:.2f}ms\")\n", + "\n", + "print(\"N has to be bigger than:\", sampling_rates / (k_min * chosen_t))\n", + "print(\"N has to be smaller than:\", sampling_rates * chosen_t)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# pulse compression\n", + "d_min_cm = 10\n", + "t_min = d_min_cm * 1e-2 / c\n", + "print(t_min)\n", + "\n", + "FS_min = 1 / t_min\n", + "print(FS_min)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "bandwidth = 3000\n", + "print(f\"pulse range resolution {c / (2 * bandwidth) * 1e3:.1f} mm\")" + ] + }, { "cell_type": "code", "execution_count": null, @@ -205,14 +308,14 @@ " else:\n", " color='w'\n", "\n", - " if matrix.dtype == np.float:\n", + " if matrix.dtype == float:\n", " ax.text(ii+df_y, jj+df_x, f\"{matrix[i, j]:.1f}\", ha='center', va='center', size=8, color=color)\n", " else:\n", " ax.text(ii+df_y, jj+df_x, f\"{matrix[i, j]:.0f}\", ha='center', va='center', size=8, color=color)\n", " \n", "fig, ax = plt.subplots()\n", "fig.set_size_inches(figsize, figsize)\n", - "matrix = matrix.astype(np.int)\n", + "matrix = matrix.astype(int)\n", "im = pcolorfast_custom(ax, freqs, freqs, matrix)\n", "add_colorbar(fig, ax, im, title='distance [cm]')\n", "#add_labels(ax, matrix)\n", @@ -246,7 +349,7 @@ "ax.set_title('distance range')\n", "#ax.legend(loc='upper left', bbox_to_anchor=[1.0, 1.0])\n", "ax.legend(loc='upper right')#, bbox_to_anchor=[1.0, 1.0])\n", - "#save_fig(fig, 'plots/theory/limits_distances.pdf')" + "save_fig(fig, 'plots/theory/limits_distances.pdf')" ] }, { @@ -420,7 +523,11 @@ "outputs": [], "source": [ "from utils.simulation import get_freq_slice_pyroom, WIDEBAND_FILE, create_wideband_signal\n", - "from crazyflie_description_py.parameters import N_BUFFER, FS\n", + "from utils.constants import PLATFORM\n", + "if PLATFORM == \"epuck\":\n", + " from epuck_description_py.parameters import N_BUFFER, FS\n", + "elif PLATFORM == \"crazyflie\":\n", + " from crazyflie_description_py.parameters import N_BUFFER, FS\n", "\n", "print(FS)\n", "\n", @@ -428,7 +535,7 @@ "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", + "max_freq = 6000\n", "min_dist = 1\n", "max_dist = 50\n", "freq_start = int(min_freq / max(frequencies) * len(frequencies))\n", @@ -448,7 +555,6 @@ "source": [ "fname = \"results/df_matrix_pyroom.pkl\"\n", "try:\n", - " raise \n", " series_all = pd.read_pickle(fname)\n", " print(\"read\", fname)\n", " np.testing.assert_allclose(series_all.distances, distances)\n", @@ -464,7 +570,7 @@ " signal = np.load(WIDEBAND_FILE)\n", " \n", " except:\n", - " print(\"creating wideband signal...\")\n", + " print(\"creating wideband signal...\", freq[:10], freq[-10:])\n", " signal = create_wideband_signal(freq)\n", " np.save(WIDEBAND_FILE, signal)\n", " print(f\"saved as {WIDEBAND_FILE}\")\n", @@ -496,48 +602,6 @@ "#matrix_slices(df_matrix, freq, dist)" ] }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "## play around with beamforming before creating interference matrix" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "from utils.geometry import Context\n", - "from utils.simulation import generate_room, get_buffers\n", - "from audio_stack.beam_former import BeamFormer\n", - "\n", - "context = Context.get_platform_setup()\n", - "beamformer = BeamFormer(mic_positions=context.mics)\n", - "\n", - "df_matrix_beamformed = np.empty((len(frequencies), len(dist)))\n", - "for j, distance_cm in enumerate(dist):\n", - " room = generate_room(distance_cm=distance_cm)\n", - " signals_f = get_buffers(room, signal, n_times=2) # 1 x 4 x n_freqs\n", - " signals_f = signals_f[0, :, :].T\n", - " \n", - " # combine mics signals with beamforming. \n", - " #coeffs = beamformer.beamform_das(np.pi/2, frequencies_hz=frequencies)\n", - " R = beamformer.get_correlation(signals_f)\n", - " coeffs = beamformer.beamform_mvdr(R, np.pi/2, frequencies_hz=frequencies)\n", - " slice_total = np.sum(np.multiply(coeffs, signals_f), axis=1)\n", - " df_matrix_beamformed[:, j] = np.abs(slice_total) ** 2\n", - " \n", - "fig, ax = plt.subplots()\n", - "ax.pcolorfast(dist, freq, df_matrix_beamformed[freq_start:freq_end, :])\n", - "gains = np.mean(df_matrix_beamformed[freq_start:freq_end, 5:], axis=1)\n", - "fig, ax = plt.subplots()\n", - "ax.pcolorfast(dist, freq, df_matrix_beamformed[freq_start:freq_end, :]/gains[:, None])\n", - "plt.figure()\n", - "plt.plot(freq, gains)" - ] - }, { "cell_type": "code", "execution_count": null, @@ -753,56 +817,11 @@ " #assert np.abs(d * 100 - distance_cm) < 1e-3" ] }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "import progressbar\n", - "results = pd.DataFrame(columns=['distance_cm', 'azimuth_deg', 'mics', 'error', 'estimate'])\n", - "distances_cm = [30] #np.arange(10, 50, step=10)\n", - "azimuths_deg = [-40] #np.arange(-180, 180, step=30)\n", - "n_mics_list = range(2, 5)\n", - "\n", - "combinations = []\n", - "for n in n_mics_list:\n", - " combinations += list(itertools.combinations(range(4), n))\n", - "n_combinations = len(combinations)\n", - "print(combinations)\n", - "\n", - "max_value = len(distances_cm) * len(azimuths_deg) * n_combinations\n", - "i = 0\n", - "with progressbar.ProgressBar(max_value=max_value) as p:\n", - " for distance_cm, azimuth_deg in itertools.product(distances_cm, azimuths_deg):\n", - " for chosen_mics in combinations:\n", - " de = simulate_distance_estimator(\n", - " chosen_mics=chosen_mics, azimuth_deg=azimuth_deg, distance_cm=distance_cm\n", - " )\n", - " d = de.get_distance_estimate() * 100\n", - " results.loc[len(results), :] = dict(\n", - " distance_cm=distance_cm,\n", - " azimuth_deg=azimuth_deg,\n", - " mics=chosen_mics,\n", - " error=abs(d-distance_cm),\n", - " estimate=d\n", - " )\n", - " p.update(i); i += 1\n", - " \n", - "import seaborn as sns\n", - "plt.figure()\n", - "i = 0\n", - "for labels, df in results.groupby(['mics', 'distance_cm']):\n", - " plt.scatter(df.azimuth_deg, df.error, label=f\"{labels}\", color=f'C{i}')\n", - " i += 1\n", - "plt.legend()" - ] - }, { "cell_type": "markdown", "metadata": {}, "source": [ - "### angle estimation" + "# Geometry study" ] }, { @@ -815,7 +834,6 @@ "source": [ "azimuth_deg = 30\n", "distance_cm = 20\n", - "distance_gt = distance_cm * 1e-2\n", "\n", "distance_estimator = simulate_distance_estimator(\n", " chosen_mics=range(4), azimuth_deg=azimuth_deg, distance_cm=distance_cm, ax=ax\n", @@ -828,7 +846,7 @@ " chosen_mics=range(4), azimuth_deg=azimuth_deg, distance_cm=distance_cm, ax=ax\n", ")\n", "thetas, probs = distance_estimator.get_angle_distribution(\n", - " distance_estimate_m=distance_gt\n", + " distance_estimate_cm=distance_cm\n", ")\n", "theta = get_estimate(thetas, probs)\n", "ax_all.scatter(thetas, probs, label=f'all, estimate={theta}')\n", @@ -840,7 +858,7 @@ " chosen_mics=[mic], azimuth_deg=azimuth_deg, distance_cm=distance_cm, ax=ax\n", " )\n", " thetas, probs = distance_estimator.get_angle_distribution(\n", - " distance_estimate_m=distance_gt\n", + " distance_estimate_cm=distance_cm\n", " )\n", " theta = get_estimate(thetas, probs)\n", " ax_all.scatter(thetas, probs, label=f'mic{mic}, estimate={theta}')\n", @@ -1127,13 +1145,6 @@ " save_fig(fig_slices, f\"plots/simulation/study_dslices_{str(sigma_y).replace('.','-')}.pdf\")" ] }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "# Geometry study" - ] - }, { "cell_type": "code", "execution_count": null, @@ -1333,6 +1344,20 @@ " return fig, axs" ] }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "name = \"amplitude_noise_new\"\n", + "try:\n", + " results_df = pd.read_pickle(f\"results/simulation/{name}.pkl\")\n", + "except:\n", + " print(f\"File {name}.pkl not found.\")\n", + " print(\"Generate results with generate_simulation_results.py\")" + ] + }, { "cell_type": "code", "execution_count": null, @@ -1343,9 +1368,6 @@ "source": [ "from utils.plotting_tools import plot_error_distance, labels\n", "\n", - "name = \"amplitude_noise_new\"\n", - "results_df = pd.read_pickle(f\"results/simulation/{name}.pkl\")\n", - "\n", "fig, axs = plot_error_distance(\n", " results_df, column=\"sigmay\", name=name + \" $\\\\sigma_y$ [-]\", aggfunc=np.nanmean\n", ")\n", @@ -1608,45 +1630,12 @@ "#plot_all_frequencies(name, logy=False, column=\"sigmarelative\", column_name=\"amplitude noise[-]\")" ] }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "# Geometry tests" - ] - }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], - "source": [ - "from utils.geometry import *\n", - "\n", - "distance_cm = 200\n", - "azimuth_deg = 70 #+ mic_idx * 90\n", - "\n", - "#context = Context.get_crazyflie_setup()\n", - "context = Context.get_standard_setup()\n", - "print(context.mics)\n", - "\n", - "for mic_idx in range(4):\n", - " #azimuth_deg = 45 + mic_idx * 90\n", - " context.source = context.mics[mic_idx] * 2\n", - " n = get_normal(distance_cm*1e-2, azimuth_deg)[:2]\n", - " \n", - " context.plot(normal=n)\n", - " \n", - " delta_m = context.get_delta(azimuth_deg, distance_cm, mic_idx)*1e-2\n", - " \n", - " distance_est = context.get_distance(delta_m*1e2, azimuth_deg, mic_idx)\n", - " azimuth_est = context.get_angles(delta_m, distance_est*1e-2, mic_idx)\n", - " \n", - " #assert delta_est == 2, delta_est\n", - " #assert source_distance == 1, source_distance\n", - " np.testing.assert_allclose(distance_est, distance_cm)\n", - " assert azimuth_deg in azimuth_est, (azimuth_est, azimuth_deg)" - ] + "source": [] } ], "metadata": { @@ -1665,7 +1654,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.8.12" + "version": "3.8.10" }, "toc": { "base_numbering": 1, diff --git a/python/generate_classifier_results.py b/python/generate_classifier_results.py index a4616b80..783d6c4c 100644 --- a/python/generate_classifier_results.py +++ b/python/generate_classifier_results.py @@ -28,6 +28,8 @@ # walls location per dataset, [distance_cm, angle_deg] WALLS_DICT = { + "2021_07_08_stepper_fast": [[1e-3, 90]], + "2021_07_27_epuck_wall": [[1e-3, 90]], "2021_10_12_flying": [[100, 90]], "2021_11_23_demo": [[100, 90]], # , [0.1, -90]], "2022_01_25": [[80, 90]], # , [0.1, -90]], @@ -85,7 +87,11 @@ def normal(angle_deg): ] data_df = pd.read_pickle(f"../datasets/{exp_name}/all_data.pkl") - row = data_df.loc[data_df.appendix == appendix].iloc[0] + try: + row = data_df.loc[data_df.appendix == appendix].iloc[0] + except: + print(f"did not find {appendix} in {data_df.appendix.unique()} for {exp_name}") + if flying: from crazyflie_description_py.parameters import FLYING_HEIGHT_CM @@ -100,6 +106,7 @@ def normal(angle_deg): walls = WALLS_DICT[exp_name] distances_wall = None # np.full(positions_cm.shape[0], np.nan) angles_wall = None # np.full(positions_cm.shape[0], np.nan) + for wall in walls: distances_here = wall[0] - positions_cm.dot(normal(wall[1])) angles_here = wall[1] - yaws_deg @@ -241,9 +248,10 @@ def generate_classifier_results(matrix_df, fname="", verbose=False): if __name__ == "__main__": + appendix = "test4" for estimator in ["particle", "moving"]: try: - matrix_df = pd.read_pickle(f"results/demo_results_matrices_{estimator}.pkl") + matrix_df = pd.read_pickle(f"results/demo_results_matrices_{estimator}{appendix}.pkl") except FileNotFoundError: print("Run generate_flying_results.py to generate results.") raise diff --git a/python/generate_filtering_results.py b/python/generate_filtering_results.py index f6fb9d26..aec5588f 100644 --- a/python/generate_filtering_results.py +++ b/python/generate_filtering_results.py @@ -4,26 +4,37 @@ generate_filtering_results.py: Calculate and save distance estimates for all measured stepper data. """ -from crazyflie_description_py.experiments import ( - WALL_ANGLE_DEG_STEPPER, - WALL_DISTANCE_CM_STEPPER, -) +import itertools import time import numpy as np import pandas as pd import progressbar -from utils.constants import SPEED_OF_SOUND -from utils.estimators import DistanceEstimator, get_estimate -from utils.inference import Inference -from utils.simulation import get_freq_slice_theory, get_freq_slice_pyroom +from utils.constants import PLATFORM + +if PLATFORM == "crazyflie": + from crazyflie_description_py.experiments import DISTANCES_CM + from crazyflie_description_py.experiments import WALL_ANGLE_DEG_STEPPER +elif PLATFORM == "epuck": + from epuck_description_py.experiments import DISTANCES_CM + from epuck_description_py.experiments import WALL_ANGLE_DEG_STEPPER + +from generate_flying_results import get_max_signals + +from utils.estimators import get_estimate +from utils.simulation import get_freq_slice_theory, get_freq_slice_pyroom, WIDEBAND_FILE +from utils.pandas_utils import save_pickle +from utils.plotting_tools import make_dirs +from utils.simulation import create_wideband_signal from utils.moving_estimators import MovingEstimator from utils.particle_estimators import ParticleEstimator +from utils.split_particle_estimators import SplitParticleEstimator +from utils.histogram_estimators import HistogramEstimator -from crazyflie_demo.wall_detection import WallDetection +from crazyflie_demo.wall_detection import WallDetection, MAG_THRESH DISCRETIZATIONS = dict( superfine=(1.0, 5), @@ -32,15 +43,21 @@ coarse=(5.0, 30), supercoarse=(10.0, 90), ) +N_PARTICLES = dict( + superfine=1600, + fine=800, + medium=400, + coarse=200, + supercoarse=100, +) PARAMETERS_ALL = dict( discretizations=["superfine", "fine", "medium", "coarse", "supercoarse"], n_windows=[1, 3, 5], methods=["raw", "theoretical", "calibrated"], + chosen_mics=[[0,1,2,3]], ) -MAG_THRESH = 1e-3 - # d_max is c / 4 * \delta_f = 330 / (4 * 62) = 1.33m for stepper experiments, # d_max is c / 4 * \delta_f = 330 / (4 * 94) = 0.91m for flying experiments DMIN = 7 @@ -48,14 +65,13 @@ CALIBRATION = "iir" ALPHA_IIR = 0.3 +BEAMFORM = False USE_PYROOMACOUSTICS = True - -def get_magnitudes(stft, mag_thresh=MAG_THRESH): +def average_signals(stft, mag_thresh=MAG_THRESH): stft[np.abs(stft) < mag_thresh] = np.nan - return np.nanmedian(np.abs(stft), axis=0) - + return np.nanmedian(stft, axis=0) def angle_error(a1_deg, a2_deg): error = (a2_deg - a1_deg) % 360 # between 0 and 360 @@ -65,20 +81,20 @@ def angle_error(a1_deg, a2_deg): else: error[error > 180] -= 360 return error - # return ( - # np.diff(np.unwrap([a1_deg / 180 * np.pi, a2_deg / 180 * np.pi]))[0] - # * 180 - # / np.pi - # ) -def generate_results(df_chosen, fname="", parameters=PARAMETERS_ALL): - def save(): - save_df = err_df.apply(pd.to_numeric, axis=0, errors="ignore") - save_df.to_pickle(fname) - print("Saved intermediate as", fname) +def generate_results(df_chosen, fname="", parameters=PARAMETERS_ALL, beamform=BEAMFORM): + if beamform: + print("running with beamforming") + else: + print("running without beamforming") + + # to ensure reproducibility for particle filter + np.random.seed(1) - def fill_distance(err_df, probs_dist, method): + def fill_distance(err_df, res, method): + probs_dist = res["prob_dist_moving"] + distances_cm = res["dist_moving"] d = get_estimate(distances_cm, probs_dist) err_df.loc[len(err_df), :] = { "error": d - distance, @@ -88,11 +104,14 @@ def fill_distance(err_df, probs_dist, method): "algorithm": algo + " distance", "runtime": runtime, "discretization": discretization, + "chosen_mics": chosen_mics } - def fill_angle(err_df, probs_angles, method): + def fill_angle(err_df, res, method): + probs_angles = res["prob_angle_moving"] + angles_deg = res["angle_moving"] a = get_estimate(angles_deg, probs_angles) - error_deg = angle_error(a, azimuth_deg) + error_deg = angle_error(a, gt_azimuth_deg) runtime = time.time() - t1 err_df.loc[len(err_df), :] = { "error": error_deg, @@ -102,6 +121,7 @@ def fill_angle(err_df, probs_angles, method): "algorithm": algo + " angle", "runtime": runtime, "discretization": discretization, + "chosen_mics": chosen_mics } def fill_random_and_fixed(err_df): @@ -114,15 +134,17 @@ def fill_random_and_fixed(err_df): algorithm=f"{algo} distance", runtime=0, discretization=discretization, + chosen_mics=chosen_mics ) err_df.loc[len(err_df), :] = dict( method="random", mic="all", distance=d, - error=np.random.choice(angles_deg) - azimuth_deg, + error=np.random.choice(angles_deg) - gt_azimuth_deg, algorithm=f"{algo} angle", runtime=0, discretization=discretization, + chosen_mics=chosen_mics ) # fill with constant zero distance estimate err_df.loc[len(err_df), :] = dict( @@ -133,6 +155,7 @@ def fill_random_and_fixed(err_df): algorithm=f"{algo} distance", runtime=0, discretization=discretization, + chosen_mics=chosen_mics ) # fill with constant zero angle estimate err_df.loc[len(err_df), :] = dict( @@ -143,13 +166,14 @@ def fill_random_and_fixed(err_df): algorithm=f"{algo} angle", runtime=0, discretization=discretization, + chosen_mics=chosen_mics ) - azimuth_deg = WALL_ANGLE_DEG_STEPPER - chosen_mics = range(4) # [0, 1, 3] - algo = "bayes" # could do cost to - use_uniform_prior = True + gt_distances_cm = DISTANCES_CM + assert len(DISTANCES_CM) == len(df_chosen.distance.unique()) + gt_azimuth_deg = WALL_ANGLE_DEG_STEPPER + algo = "bayes" # bayes or cost err_df = pd.DataFrame( columns=[ "method", @@ -159,136 +183,138 @@ def fill_random_and_fixed(err_df): "algorithm", "runtime", "discretization", + "chosen_mics" ] ) - inf_machine = Inference() - df_chosen.sort_values("distance", ascending=False, inplace=True) - distances = df_chosen.distance.values - df_chosen.loc[:, "distance"] += WALL_DISTANCE_CM_STEPPER - all_magnitudes = np.abs(np.concatenate([*df_chosen.stft.values])) - calibration_magnitudes = np.median(all_magnitudes, axis=0) - frequencies = df_chosen.iloc[0].frequencies_matrix[0, :] - - # dmax = 1e2 * SPEED_OF_SOUND / (4 * np.min(np.diff(frequencies))) - - inf_machine.add_geometry([min(distances), max(distances)], azimuth_deg) for discretization in parameters["discretizations"]: step_cm, step_deg = DISCRETIZATIONS[discretization] print(f"----------------- discretization {discretization} -------------------") - distances_cm = np.arange(DMIN, DMAX, step=step_cm) angles_deg = np.arange(360, step=step_deg) - n_particles = len(distances_cm) * len(angles_deg) // 2 + n_particles = N_PARTICLES[discretization] print(f"Nd = {len(distances_cm)}, Na = {len(angles_deg)} -> Np = {n_particles}") - if USE_PYROOMACOUSTICS: - # from utils.simulation import create_wideband_signal - # signal = create_wideband_signal(frequencies) - signal = np.load("results/wideband.npy") + WallDetection.CALIBRATION = CALIBRATION # fixed, iir, window + WallDetection.N_CALIBRATION = 2 # for iir, at least two. + WallDetection.ALPHA_IIR = ALPHA_IIR + WallDetection.MASK_BAD = None + WallDetection.SIMPLIFY_ANGLES = False + WallDetection.BEAMFORM = beamform + WallDetection.N_PARTICLES = n_particles for method in parameters["methods"]: print("running", method) - - estimator_dict = { - f"histogram {n_window}": MovingEstimator( - n_window=n_window, distances_cm=distances_cm, angles_deg=angles_deg - ) - for n_window in parameters["n_windows"] - } - estimator_dict[f"particle {n_particles}"] = ParticleEstimator( - n_particles=n_particles, - global_=False, - distances_cm=distances_cm, - angles_deg=angles_deg, - ) - - WallDetection.CALIBRATION = CALIBRATION # fixed, iir, window - WallDetection.N_CALIBRATION = 2 # for iir, at least two. - WallDetection.ALPHA_IIR = ALPHA_IIR - WallDetection.MASK_BAD = None - WallDetection.SIMPLIFY_ANGLES = False - wall_detection_dict = { - key: WallDetection(python_only=True, estimator=value) - for key, value in estimator_dict.items() - } - - p = progressbar.ProgressBar(maxval=len(distances)) - p.start() - - for i_d, distance in enumerate(distances): - if method == "theoretical": - - if USE_PYROOMACOUSTICS: - magnitudes = get_freq_slice_pyroom( - frequencies, - distance, - azimuth_deg=azimuth_deg, - chosen_mics=chosen_mics, - signal=signal, - ) - magnitudes = magnitudes.T + for chosen_mics in parameters["chosen_mics"]: + estimator_dict = {} + for n_window in parameters["n_windows"]: + estimator_dict[f"histogram {n_window}"] = MovingEstimator( + n_window=n_window, distances_cm=distances_cm, angles_deg=angles_deg + ) + estimator_dict[f"particle {n_particles}"] = "particle" + ##estimator_dict["histogram"] = HistogramEstimator( + # distances_cm=distances_cm, + # angles_deg=angles_deg + #) + + wall_detection_dict = {} + for key, value in estimator_dict.items(): + wall_detection_dict[key] = WallDetection(python_only=True, estimator=value) + + print(f"Using mics: {chosen_mics}") + p = progressbar.ProgressBar(maxval=len(df_chosen.distance)) + p.start() + for i, (distance_wrong, df_dist) in enumerate(df_chosen.groupby("distance", sort=False)): + distance = gt_distances_cm[i] + + # need to read signal to find frequencies + stft_exp = df_dist.loc[:, "stft"].iloc[0] # verified: 3 x 4 x 34 + if PLATFORM == "epuck": + signals_f, frequencies = get_max_signals(stft_exp[:, chosen_mics, :], df_dist.loc[:, "frequencies_matrix"].iloc[0]) + signals_f = signals_f.T else: - magnitudes = get_freq_slice_theory( - frequencies, - distance, - azimuth_deg=azimuth_deg, - chosen_mics=chosen_mics, + signals_f = average_signals(stft_exp)[chosen_mics, :] + frequencies = df_dist.iloc[0].frequencies_matrix[0, :] + + #print("wrong:", distance_wrong, "right:", distance) + if method == "theoretical": + if USE_PYROOMACOUSTICS: + signal = np.load(WIDEBAND_FILE) + signals_f = get_freq_slice_pyroom( + frequencies, + distance, + azimuth_deg=gt_azimuth_deg, + chosen_mics=chosen_mics, + signal=signal, + return_complex=True + ) + else: + if beamform: + raise ValueError("Beamforming and theoretical simulation not supported. Use pyroom instead") + signals_f = get_freq_slice_theory( + frequencies, + distance, + azimuth_deg=gt_azimuth_deg, + chosen_mics=chosen_mics, + ) + signals_f = np.sqrt(signals_f.T) # now it's 4x32 + + position_cm = [0, -distance, 50] + + yaw_deg = 0 #90 - WALL_ANGLE_DEG_STEPPER + for name, wall_detection in wall_detection_dict.items(): + t1 = time.time() + res = wall_detection.listener_callback_offline( + signals_f, frequencies, position_cm, yaw_deg, chosen_mics=chosen_mics ) - magnitudes = np.sqrt(magnitudes.T) # now it's 4x32 - else: - stft_exp = df_chosen.loc[ - df_chosen.distance == distance, "stft" - ].iloc[0] - magnitudes = get_magnitudes(stft_exp)[chosen_mics, :] - - position_cm = [0, -distance, 50] - yaw_deg = 0 - for name, wall_detection in wall_detection_dict.items(): - t1 = time.time() - ( - __, - __, - probs_dist, - probs_angles, - ) = wall_detection.listener_callback_offline( - magnitudes.T, frequencies, position_cm, yaw_deg - ) - runtime = time.time() - t1 - fill_distance(err_df, probs_dist, method=f"{method} {name}") - fill_angle(err_df, probs_angles, method=f"{method} {name}") + if res is None: + print("problem with",position_cm, yaw_deg, chosen_mics) + continue + #print(res["prob_dist_static"][:10]) + probs_dist = res["prob_dist_moving"] + probs_angles = res["prob_angle_moving"] + runtime = time.time() - t1 + fill_distance(err_df, res, method=f"{method} {name}") + fill_angle(err_df, res, method=f"{method} {name}") - p.update(i_d) + p.update(i) - if fname != "": - save() + if fname != "": + save_pickle(err_df, fname) fill_random_and_fixed(err_df) if fname != "": - save() + save_pickle(err_df, fname) if __name__ == "__main__": - np.random.seed(1) + from utils.custom_argparser import exp_parser, check_platform - exp_name = "2021_07_08_stepper_fast" - motors = "all45000" - bin_selection = 5 + parser = exp_parser( + "Apply the moving or particle estimator to flying or stepper datasets." + ) + args = parser.parse_args() + + check_platform(args) + + if PLATFORM == "crazyflie": + exp_name = "2021_07_08_stepper_fast" + motors = "all45000" + bin_selection = 5 + name = "stepper" + best_mics = [[0,1,2,3]] + elif PLATFORM == "epuck": + #raise NotImplementedError("Cannot run this script because of bin_selection==0 for epuck") + exp_name = "2021_07_27_epuck_wall" + motors = "sweep_and_move" + bin_selection = 0 + name = "epuck" + best_mics = [[0,1,2,3]] df_all = pd.read_pickle(f"../datasets/{exp_name}/all_data.pkl") - df_chosen = df_all.loc[ - (df_all.motors == motors) & (df_all.bin_selection == bin_selection) - ].copy() - - parameters = dict( - discretizations=["superfine", "fine", "medium", "coarse", "supercoarse"], - n_windows=[1, 3, 5], - methods=["calibrated"], - ) - fname = "results/stepper_results_timing.pkl" - # generate_results(df_chosen, fname=fname, parameters=parameters) + df_all = df_all.reindex(index=df_all.index[::-1]) # reverse to emulate wall approach parameters = dict( discretizations=[ @@ -297,12 +323,32 @@ def fill_random_and_fixed(err_df): "medium", "coarse", "supercoarse", - ], # ["superfine", "fine", "medium", "coarse", "supercoarse"], - n_windows=[1, 3, 5], - methods=["theoretical", "calibrated"], + ], + #n_windows=[1, 3, 5], + n_windows=[], + methods=["calibrated", "theoretical"], + chosen_mics=best_mics, ) - # fname = "results/stepper_results_online_new.pkl" # non-uniform - # fname = "results/stepper_results_online_new_uniform.pkl" - # fname = "results/stepper_results_online_seed1.pkl" - fname = "results/stepper_results_online_seed1_uniform.pkl" - generate_results(df_chosen, fname=fname, parameters=parameters) + df_chosen = df_all.loc[ + (df_all.motors == motors) & (df_all.bin_selection == bin_selection) + ].copy() + if BEAMFORM: + fname = f"results/{name}_results_beamform.pkl" + else: + fname = f"results/{name}_results.pkl" + generate_results(df_chosen, fname=fname, parameters=parameters, beamform=BEAMFORM) + + parameters = dict( + discretizations=["fine"], + n_windows=[], # use only particle + methods=["calibrated"], + chosen_mics= [m for n in range(1, 5) for m in itertools.combinations(range(4), n)] + ) + if BEAMFORM: + fname = f"results/{fname}_results_mics_ablation_beamform.pkl" + else: + fname = f"results/{name}_results_mics_ablation.pkl" + df_chosen = df_all.loc[ + (df_all.motors == motors) & (df_all.bin_selection == bin_selection) + ].copy() + generate_results(df_chosen, fname=fname, parameters=parameters, beamform=BEAMFORM) diff --git a/python/generate_flying_results.py b/python/generate_flying_results.py index 2cb54e19..0f665865 100644 --- a/python/generate_flying_results.py +++ b/python/generate_flying_results.py @@ -5,43 +5,117 @@ import pandas as pd from progressbar import ProgressBar -from crazyflie_demo.wall_detection import WallDetection +from crazyflie_demo.wall_detection import WallDetection, MAG_THRESH + +from utils.constants import PLATFORM from utils.moving_estimators import get_estimate -DATASET = "flying" -# DATASET = "stepper" -# DATASET = "demo" +#SIMULATE = "pyroom" +SIMULATE = "" -ESTIMATORS = ["particle"] # ["moving", "particle"] # or moving -ESTIMATORS = ["moving"] # ["moving", "particle"] # or moving +#ESTIMATORS = ["moving"] +ESTIMATORS = ["particle"] +#ESTIMATORS = ["particle", "histogram", "moving"] -REL_STD = 0.0 # was found not to have a significant influce on results. +def get_max_signals(stft, frequencies_matrix): + signals_f_list = [] + freqs_list = [] + for t in range(stft.shape[0]): + freq_bin = np.argmax(np.sum(np.abs(stft[t]), axis=0)) # sum across mics + f = frequencies_matrix[t, freq_bin] + #print(f"amplitude of {f}", np.sum(np.abs(stft[t, :, freq_bin]), axis=0)) + freqs_list.append(f) + signals_f_list.append(stft[t, :, freq_bin]) + signals_f = np.array(signals_f_list) # freqs x 4 + freqs = np.array(freqs_list) + return signals_f, freqs +def combine_stepper_df(data_df, motors="all45000", bin_selection=5, merge="average", platform=PLATFORM): + """Combine all rows of a stepper dataset to one, as in flying datasets.""" + if platform == "crazyflie": + print("crazyflie platform") + from crazyflie_description_py.experiments import WALL_ANGLE_DEG_STEPPER, DISTANCES_CM + height = 0.5 + elif platform == "epuck": + print("epuck platform") + from epuck_description_py.experiments import WALL_ANGLE_DEG_STEPPER, DISTANCES_CM + height = 0.5 # to emulate flying -def combine_stepper_df(data_df, motors="all45000", bin_selection=5): - """ combine all rows of a stepper dataset to one, as in flying datasets.""" chosen_df = data_df.loc[ (data_df.motors == motors) & (data_df.bin_selection == bin_selection) - ] + ][::-1] # reverse, to emulate wall approach. data_row = pd.Series(index=chosen_df.columns, dtype=object) - data_row.positions = np.concatenate( - [np.r_[[[0, -distance, 50, 0]]] for distance in chosen_df.distance.values] - ) - data_row.stft = np.concatenate( - [np.r_[[np.median(stft, axis=0)]] for stft in chosen_df.stft.values] - ) - data_row.frequencies_matrix = np.concatenate( - [ - np.r_[[frequencies_mat[0, :]]] - for frequencies_mat in chosen_df.frequencies_matrix.values - ], - axis=0, - ) - data_row.seconds = chosen_df.seconds.values + + #yaw_deg = 90 - WALL_ANGLE_DEG_STEPPER + yaw_deg = 0 + + # assumption: stft of all data has to be of the same shape!! + first_stft = chosen_df.iloc[0].stft + n_mics = first_stft.shape[1] + if bin_selection == 5: + # at each time, all frequencies are played. + n_freqs = first_stft.shape[2] + else: + # at each time, a new frequency is played. + n_freqs = first_stft.shape[0] + + data_row.positions = np.empty((0, 4)) + data_row.stft = np.empty((0, n_mics, n_freqs)) + data_row.frequencies_matrix = np.empty((0, n_freqs)) + data_row.seconds = np.empty(0) + for i, (distance_wrong, df) in enumerate(chosen_df.groupby("distance", sort=False)): + assert len(df) == 1 + row = df.iloc[0] + n_times = row.stft.shape[0] + + distance_cm = DISTANCES_CM[i] + + # with below values, we assume that wall is north and horizontal and the movement is vertical downwards. + new_positions = np.array([0, -distance_cm*1e-2, height, yaw_deg]).reshape(1, -1) + + if bin_selection == 5: + new_frequencies = row.frequencies_matrix[[0], :] # should be the same! + + if merge == "average": + valid_stft = row.stft.copy() + valid_stft[valid_stft < MAG_THRESH] = np.nan + new_stft = np.nanmedian(valid_stft, axis=0)[None, :, :] # 1 x 4 x 32 + new_seconds = row.seconds[[0]] + elif merge == "middle": + middle = row.stft.shape[0] // 2 + new_stft = row.stft[[middle], :, :] # 1 x 4 x 32 + new_seconds = row.seconds[[middle]] + elif merge == "all": + new_stft = row.stft # 3 x 4 x 32 + new_seconds = row.seconds + new_positions = np.tile(new_positions, (n_times, 1)) + + # for e-puck, we still use bin_selection 0, so we need to get the best bin at each time. + elif bin_selection == 0: + signals_f, freqs = get_max_signals(row.stft, row.frequencies_matrix) + new_stft = signals_f.T[None, :, :] + new_frequencies = freqs[None, :] + new_seconds = [row.seconds[0]] + + data_row.positions = np.concatenate( + [data_row.positions, new_positions], axis=0 + ) + data_row.frequencies_matrix = np.concatenate( + [data_row.frequencies_matrix, new_frequencies], axis=0 + ) + data_row.stft = np.concatenate( + [data_row.stft, new_stft], axis=0 + ) + data_row.seconds = np.concatenate([data_row.seconds, new_seconds], axis=0) return data_row +def generate_matrix_results(data_row, parameters, estimator, fname="", verbose=False, beamform=False, platform=PLATFORM): + from utils.simulation import get_freq_slice_pyroom, get_freq_slice_theory, WIDEBAND_FILE + if platform == "crazyflie": + from crazyflie_description_py.experiments import WALL_ANGLE_DEG_STEPPER + elif platform == "epuck": + from epuck_description_py.experiments import WALL_ANGLE_DEG_STEPPER -def generate_matrix_results(data_row, parameters, estimator, fname="", verbose=False): n_rows = len(list(itertools.product(*parameters.values()))) result_df = pd.DataFrame( columns=[ @@ -50,22 +124,20 @@ def generate_matrix_results(data_row, parameters, estimator, fname="", verbose=F "mask bad", "outlier factor", "n window", - "relative std", "simplify angles", "matrix distances", "matrix angles", "distances_cm", "angles_deg", + "gt_distances_cm", + "gt_angles_deg", ], index=range(n_rows), ) counter = 0 - progressbar = ProgressBar(maxval=n_rows) - progressbar.start() - for calib, mask_bad, n_window, std, simplify in itertools.product( + for calib, mask_bad, n_window, simplify in itertools.product( *parameters.values() ): - # print("experiment", calib, mask_bad, n_window, std, simplify) WallDetection.CALIBRATION = calib[0] if calib[0] == "iir": WallDetection.N_CALIBRATION = 2 @@ -75,20 +147,20 @@ def generate_matrix_results(data_row, parameters, estimator, fname="", verbose=F WallDetection.MASK_BAD = mask_bad[0] WallDetection.OUTLIER_FACTOR = mask_bad[1] WallDetection.N_WINDOW = n_window - WallDetection.RELATIVE_MOVEMENT_STD = std WallDetection.SIMPLIFY_ANGLES = simplify + WallDetection.BEAMFORM = beamform wall_detection = WallDetection(python_only=True, estimator=estimator) + wall_detection.print_params() - result_df.loc[counter, "calibration name"] = calib[0] - result_df.loc[counter, "calibration param"] = calib[1] - result_df.loc[counter, "mask bad"] = str(mask_bad[0]) - result_df.loc[counter, "outlier factor"] = mask_bad[1] - result_df.loc[counter, "n window"] = n_window - result_df.loc[counter, "simplify angles"] = simplify - result_df.loc[counter, "relative std"] = std - result_df.loc[counter, "distances_cm"] = wall_detection.estimator.distances_cm - result_df.loc[counter, "angles_deg"] = wall_detection.estimator.angles_deg + result_df.at[counter, "calibration name"] = calib[0] + result_df.at[counter, "calibration param"] = calib[1] + result_df.at[counter, "mask bad"] = str(mask_bad[0]) + result_df.at[counter, "outlier factor"] = mask_bad[1] + result_df.at[counter, "n window"] = n_window + result_df.at[counter, "simplify angles"] = simplify + result_df.at[counter, "distances_cm"] = wall_detection.estimator.distances_cm + result_df.at[counter, "angles_deg"] = wall_detection.estimator.angles_deg matrix_distances = None matrix_angles = None @@ -97,37 +169,68 @@ def generate_matrix_results(data_row, parameters, estimator, fname="", verbose=F freqs = freqs_all[freqs_all > 0] n_positions = data_row.positions.shape[0] times = [] + gt_distances = [] + gt_angles = [] + progressbar = ProgressBar(maxval=n_positions) + progressbar.start() for i in range(n_positions): - # print(f"{i+1} / {n_positions}") position_cm = data_row.positions[i, :3] * 1e2 - - signals_f = data_row.stft[i, :, freqs_all > 0] yaw_deg = data_row.positions[i, 3] time = data_row.seconds[i] + if SIMULATE == "pyroom": + signal = np.load(WIDEBAND_FILE) + signals_f = get_freq_slice_pyroom( + freqs, + -position_cm[1], + azimuth_deg=WALL_ANGLE_DEG_STEPPER, + signal=signal, + return_complex=True + ) + signals_f = signals_f.T + elif SIMULATE == "theory": + signals_f = get_freq_slice_theory( + freqs, + -position_cm[1], + azimuth_deg=WALL_ANGLE_DEG_STEPPER, + ) + signals_f = np.sqrt(signals_f) # now it's 4x32 + elif SIMULATE == "": + signals_f = data_row.stft[i, :, freqs_all > 0] + else: + raise ValueError(SIMULATE) + res = wall_detection.listener_callback_offline( - signals_f, freqs, position_cm, yaw_deg, timestamp=time, + signals_f.T, + freqs, + position_cm, + yaw_deg, + timestamp=time, ) if res is None: # if flight check did not pass, for instance continue - distances_cm_raw, prob_raw_dist, prob_moving_dist, prob_moving_angle = res + gt_distances.append(-position_cm[1]) + gt_angles.append(WALL_ANGLE_DEG_STEPPER) + if matrix_distances is None: - matrix_distances = prob_moving_dist.reshape((-1, 1)) + matrix_distances = res["prob_dist_moving"].reshape((-1, 1)) else: matrix_distances = np.c_[ - matrix_distances, prob_moving_dist.reshape((-1, 1)) + matrix_distances, res["prob_dist_moving"].reshape((-1, 1)) ] if matrix_angles is None: - matrix_angles = prob_moving_angle.reshape((-1, 1)) + matrix_angles = res["prob_angle_moving"].reshape((-1, 1)) else: - matrix_angles = np.c_[matrix_angles, prob_moving_angle.reshape((-1, 1))] + matrix_angles = np.c_[matrix_angles, res["prob_angle_moving"].reshape((-1, 1))] + progressbar.update(i) - result_df.loc[counter, "matrix distances"] = [matrix_distances] - result_df.loc[counter, "matrix angles"] = [matrix_angles] - result_df.loc[counter, "average_time"] = np.mean(times) + result_df.at[counter, "matrix distances"] = matrix_distances + result_df.at[counter, "matrix angles"] = matrix_angles + result_df.at[counter, "average_time"] = np.mean(times) + result_df.at[counter, "gt_distances_cm"] = np.array(gt_distances) + result_df.at[counter, "gt_angles_deg"] = np.array(gt_angles) counter += 1 - progressbar.update(counter) if fname != "": result_df.to_pickle(fname) print(f"Saved intermediate {counter}/{n_rows} as {fname}") @@ -135,85 +238,110 @@ def generate_matrix_results(data_row, parameters, estimator, fname="", verbose=F if __name__ == "__main__": + np.random.seed(1) # to reproduce particle filter results - np.random.seed(1) + from utils.custom_argparser import exp_parser, check_platform - if DATASET == "demo": - exp_name = "2022_01_27_demo" - appendix = "test4" - data_df = pd.read_pickle(f"../datasets/{exp_name}/all_data.pkl") - data_row = data_df.loc[data_df.appendix == appendix, :].iloc[0] + parser = exp_parser( + "Apply the moving or particle estimator to flying or stepper datasets." + ) + args = parser.parse_args() - for estimator in ESTIMATORS: - parameters = { - "calibration": [ # [("iir", 0.2)], - ("iir", alpha_iir) for alpha_iir in [0.3] - ] - + [("window", n_calib) for n_calib in [7]], - # + [("fixed", n_calib) for n_calib in np.arange(1, 10, step=2)], - "mask_bad": [ - ("fixed", None), - # ("adaptive", 2), - # ("adaptive", 4), - # ("adaptive", 10), - # (None, None), - ], - "n_window": [1] if estimator == "particle" else [1, 3, 5], - "std": [REL_STD], # only for moving - "simplify": [True, False], - } - fname = f"results/demo_results_matrices_{estimator}{appendix}.pkl" - matrix_df = generate_matrix_results( - data_row, parameters, fname=fname, verbose=True, estimator=estimator - ) - elif DATASET == "flying": - exp_name = "2021_10_12_flying" - # appendix = "_new3" - # appendix = "_new6" - for appendix in [f"_{i}" for i in range(1, 7)] + ["_new3", "_new6"]: + check_platform(args) + + beamform = False + + for exp_name in args.experiment_names: + if exp_name == "2022_01_27_demo": + data_df = pd.read_pickle(f"../datasets/{exp_name}/all_data.pkl") + for appendix in ["test3", "test4"]: + data_row = data_df.loc[data_df.appendix == appendix, :].iloc[0] + + for estimator in ESTIMATORS: + parameters = { + "calibration": [("iir", 0.3)] + + [("window", n_calib) for n_calib in [7]], + "mask_bad": [ + ("fixed", None), + ], + "n_window": [1] if estimator != "moving" else [1, 3, 5], + "simplify": [True, False], + } + if beamform: + fname = f"results/demo_results_matrices_{estimator}{appendix}_beamform_new.pkl" + else: + fname = f"results/demo_results_matrices_{estimator}{appendix}_new.pkl" + matrix_df = generate_matrix_results( + data_row, + parameters, + fname=fname, + verbose=True, + estimator=estimator, + beamform=beamform, + ) + elif exp_name == "2021_10_12_flying": data_df = pd.read_pickle(f"../datasets/{exp_name}/all_data.pkl") - data_row = data_df.loc[data_df.appendix == appendix, :].iloc[0] + #for appendix in data_df.appendix.unique():#[f"_{i}" for i in range(1, 7)] + ["_new3", "_new6"]: + for appendix in [f"_{i}" for i in range(1, 7)] + ["_new3", "_new6"]: + data_row = data_df.loc[data_df.appendix == appendix, :].iloc[0] + for estimator in ESTIMATORS: + parameters = { + "calibration": [("iir", 0.3)], + "mask_bad": [("fixed", None)], + "n_window": [1] if estimator == "particle" else [5], + "simplify": [False], + } + if beamform: + fname = f"results/flying_results_matrices_{estimator}{appendix}_beamform_new.pkl" + else: + fname = f"results/flying_results_matrices_{estimator}{appendix}_new.pkl" + matrix_df = generate_matrix_results( + data_row, + parameters, + fname=fname, + verbose=True, + estimator=estimator, + beamform=beamform, + ) + elif exp_name == "2021_07_08_stepper_fast": + appendix = "" + df_all = pd.read_pickle(f"../datasets/{exp_name}/all_data.pkl") + #data_row = combine_stepper_df(df_all, motors="all45000", bin_selection=5, merge="all") + data_row = combine_stepper_df(df_all, motors="all45000", bin_selection=5, merge="middle") for estimator in ESTIMATORS: parameters = { - "calibration": [ # [("iir", 0.2)], - ("iir", alpha_iir) for alpha_iir in [0.3] # [0.3, 0.5, 0.7] - ], - # + [("window", n_calib) for n_calib in [5, 7]], - "mask_bad": [("fixed", None),], - "n_window": [1] if estimator == "particle" else [5], # [1, 3, 5], - "std": [REL_STD], - "simplify": [True, False], + "calibration": [("iir", 0.3)], + "mask_bad": [(None, None)], + "n_window": [1] if estimator == "particle" else [5], + "simplify": [False], } - fname = f"results/flying_results_matrices_{estimator}{appendix}.pkl" + if beamform: + fname = f"results/stepper_results_matrices_{estimator}{appendix}{SIMULATE}_beamform_new.pkl" + else: + fname = f"results/stepper_results_matrices_{estimator}{appendix}{SIMULATE}_new.pkl" matrix_df = generate_matrix_results( - data_row, parameters, fname=fname, verbose=True, estimator=estimator + data_row, parameters, fname=fname, verbose=True, estimator=estimator, beamform=beamform, ) - elif DATASET == "stepper": - exp_name = "2021_07_08_stepper_fast" - appendix = "" - data_df = pd.read_pickle(f"../datasets/{exp_name}/all_data.pkl") - data_row = combine_stepper_df(data_df, motors="all45000", bin_selection=5) - for estimator in ESTIMATORS: - parameters = { - "calibration": [ # [("iir", 0.2)], - ("iir", alpha_iir) for alpha_iir in np.arange(0.1, 1.0, step=0.2) - ] - + [("window", n_calib) for n_calib in np.arange(1, 10, step=2)] - + [("fixed", n_calib) for n_calib in np.arange(1, 10, step=2)], - "mask_bad": [ - ("fixed", None), - # ("adaptive", 2), - # ("adaptive", 4), - # ("adaptive", 10), - # (None, None), - ], - "n_window": [1] if estimator == "particle" else [1, 3, 5], - "std": [REL_STD], # only used for "moving" - "simplify": [False], - } - fname = "results/stepper_results_matrices_{estimator}{appendix}.pkl" - matrix_df = generate_matrix_results( - data_row, parameters, fname=fname, verbose=True, estimator=estimator + elif exp_name == "2021_07_27_epuck_wall": + assert PLATFORM == "epuck" + appendix = "" + data_df = pd.read_pickle(f"../datasets/{exp_name}/all_data.pkl") + data_row = combine_stepper_df(data_df, motors="sweep_and_move", bin_selection=0) + for estimator in ESTIMATORS: + parameters = { + "calibration": [("iir", 0.3)], + "mask_bad": [("fixed_epuck", None)], + "n_window": [1] if estimator == "particle" else [5], + "simplify": [False], + } + if beamform: + fname = f"results/epuck_results_matrices_{estimator}{appendix}{SIMULATE}_beamform_new.pkl" + else: + fname = f"results/epuck_results_matrices_{estimator}{appendix}{SIMULATE}_new.pkl" + matrix_df = generate_matrix_results( + data_row, parameters, fname=fname, verbose=True, estimator=estimator, beamform=beamform, + ) + else: + raise ValueError( + "Only use one of the allowed datasets: 2022_01_27_demo, 2021_07_08_stepper_fast, 2021_10_12_flying" ) - else: - raise ValueError(DATASET) diff --git a/python/generate_simulation_results.py b/python/generate_simulation_results.py index 3877dc5f..7a21b472 100644 --- a/python/generate_simulation_results.py +++ b/python/generate_simulation_results.py @@ -14,6 +14,7 @@ from utils.geometry import get_deltas_from_global from utils.inference import get_probability_cost, get_probability_bayes from utils.inference import get_approach_angle_fft, get_approach_angle_cost +from utils.pandas_utils import save_pickle from utils.simulation import get_df_theory_simple MIC_IDX = 1 @@ -26,7 +27,6 @@ # copied from firmware # [3002, 3061, 3123, 3186, 3247, 3309, 3375, 3435, 3564, 3624, 3878, 3941, 3996, 4062, 4121, 4191, 4254, 4307, 4373, 4441] - def simulate_frequency_slice( distances_cm, frequencies, sigmas_delta_cm, sigmas_f, sigmas_y, n_instances ): @@ -59,60 +59,61 @@ def simulate_frequency_slice( ) i = 0 - with progressbar.ProgressBar(max_value=n_total) as p: - for distance_cm, delta_m in zip(distances_cm, deltas_m): - for (sigma_delta_cm, sigma_f, sigma_y) in itertools.product( - sigmas_delta_cm, sigmas_f, sigmas_y - ): - for counter in range(n_instances): - delta_m_noisy = ( - delta_m + np.random.normal(scale=sigma_delta_cm) * 1e-2 - ) - frequencies_noisy = frequencies + np.random.normal( - scale=sigma_f, size=len(frequencies) - ) + p = progressbar.ProgressBar(maxval=n_total) + p.start() + for distance_cm, delta_m in zip(distances_cm, deltas_m): + for (sigma_delta_cm, sigma_f, sigma_y) in itertools.product( + sigmas_delta_cm, sigmas_f, sigmas_y + ): + for counter in range(n_instances): + delta_m_noisy = ( + delta_m + np.random.normal(scale=sigma_delta_cm) * 1e-2 + ) + frequencies_noisy = frequencies + np.random.normal( + scale=sigma_f, size=len(frequencies) + ) - slice_f = get_df_theory_simple( - delta_m_noisy, frequencies_noisy, flat=True, d0=d0, - ) - slice_f += np.random.normal(scale=sigma_y, size=len(slice_f)) - - for method in METHODS: - if method == "fft": - dist, probs, _ = get_probability_bayes( - slice_f, - frequencies_noisy, - mic_idx=MIC_IDX, - distance_range=[ - min(distances_grid), - max(distances_grid), - ], - azimuth_deg=AZIMUTH_DEG, - interpolate=False, - ) - elif method == "cost": - probs = get_probability_cost( - slice_f, - frequencies_noisy, - distances_grid, - mic_idx=MIC_IDX, - ) - dist = distances_grid - - d_estimate = dist[np.argmax(probs)] - error = np.abs(d_estimate - distance_cm) - - results_df.loc[len(results_df), :] = dict( - counter=counter, - distance=distance_cm, - sigmadelta=sigma_delta_cm, - sigmaf=sigma_f, - sigmay=sigma_y, - method=method, - error=error, + slice_f = get_df_theory_simple( + delta_m_noisy, frequencies_noisy, flat=True, d0=d0, + ) + slice_f += np.random.normal(scale=sigma_y, size=len(slice_f)) + + for method in METHODS: + if method == "fft": + dist, probs, _ = get_probability_bayes( + slice_f, + frequencies_noisy, + mic_idx=MIC_IDX, + distance_range=[ + min(distances_grid), + max(distances_grid), + ], + azimuth_deg=AZIMUTH_DEG, + interpolate=False, ) - p.update(i) - i += 1 + elif method == "cost": + probs = get_probability_cost( + slice_f, + frequencies_noisy, + distances_grid, + mic_idx=MIC_IDX, + ) + dist = distances_grid + + d_estimate = dist[np.argmax(probs)] + error = np.abs(d_estimate - distance_cm) + + results_df.loc[len(results_df), :] = dict( + counter=counter, + distance=distance_cm, + sigmadelta=sigma_delta_cm, + sigmaf=sigma_f, + sigmay=sigma_y, + method=method, + error=error, + ) + p.update(i) + i += 1 results_df = results_df.apply(pd.to_numeric, errors="ignore", axis=1) return results_df @@ -157,62 +158,63 @@ def simulate_distance_slice( ) i = 0 - with progressbar.ProgressBar(max_value=n_total) as p: - for (gamma_deg, sigma_relative_cm, sigma_y, frequency,) in itertools.product( - gammas_deg, sigmas_relative_cm, sigmas_y, frequencies - ): - for counter in range(n_instances): - relative_cm_noisy = relative_distances_cm + np.random.normal( - scale=sigma_relative_cm, size=len(relative_distances_cm) - ) - start_distance_random = start_distance_cm + np.random.uniform(-10, 10) - distances_cm = start_distance_random - relative_cm_noisy * np.sin( - gamma_deg / 180 * np.pi - ) - deltas_m_noisy, d0 = get_deltas_from_global( - AZIMUTH_DEG, distances_cm, MIC_IDX - ) - - slice_d = get_df_theory_simple( - deltas_m_noisy, frequency, flat=True, d0=d0 - ) - slice_d += np.random.normal(scale=sigma_y, size=len(slice_d)) - - for method in METHODS: - if method == "fft": - gammas, probs = get_approach_angle_fft( - slice_d, - frequency, - relative_distances_cm, - bayes=True, - reduced=True, - ) - elif method == "cost": - probs = get_approach_angle_cost( - slice_d, - frequency, - relative_distances_cm, - start_distances_grid, - gammas_grid, - mic_idx=MIC_IDX, - ) # is of shape n_start_distances x n_gammas_grid - gammas = gammas_grid - - gamma_estimate = gammas[np.argmax(probs)] - error = np.abs(gamma_estimate - gamma_deg) + p = progressbar.ProgressBar(maxval=n_total) + p.start() + for (gamma_deg, sigma_relative_cm, sigma_y, frequency,) in itertools.product( + gammas_deg, sigmas_relative_cm, sigmas_y, frequencies + ): + for counter in range(n_instances): + relative_cm_noisy = relative_distances_cm + np.random.normal( + scale=sigma_relative_cm, size=len(relative_distances_cm) + ) + start_distance_random = start_distance_cm + np.random.uniform(-10, 10) + distances_cm = start_distance_random - relative_cm_noisy * np.sin( + gamma_deg / 180 * np.pi + ) + deltas_m_noisy, d0 = get_deltas_from_global( + AZIMUTH_DEG, distances_cm, MIC_IDX + ) - results_df.loc[len(results_df), :] = dict( - counter=counter, - gamma=gamma_deg, - startdistance=start_distance_random, - frequency=frequency, - sigmarelative=sigma_relative_cm, - sigmay=sigma_y, - method=method, - error=error, + slice_d = get_df_theory_simple( + deltas_m_noisy, frequency, flat=True, d0=d0 + ) + slice_d += np.random.normal(scale=sigma_y, size=len(slice_d)) + + for method in METHODS: + if method == "fft": + gammas, probs = get_approach_angle_fft( + slice_d, + frequency, + relative_distances_cm, + bayes=True, + reduced=True, ) - p.update(i) - i += 1 + elif method == "cost": + probs = get_approach_angle_cost( + slice_d, + frequency, + relative_distances_cm, + start_distances_grid, + gammas_grid, + mic_idx=MIC_IDX, + ) # is of shape n_start_distances x n_gammas_grid + gammas = gammas_grid + + gamma_estimate = gammas[np.argmax(probs)] + error = np.abs(gamma_estimate - gamma_deg) + + results_df.loc[len(results_df), :] = dict( + counter=counter, + gamma=gamma_deg, + startdistance=start_distance_random, + frequency=frequency, + sigmarelative=sigma_relative_cm, + sigmay=sigma_y, + method=method, + error=error, + ) + p.update(i) + i += 1 results_df = results_df.apply(pd.to_numeric, errors="ignore", axis=1) return results_df @@ -227,33 +229,33 @@ def compare_timing(n_instances): distances_grid = np.arange(10, 100) times = {"fft": [], "cost": []} - with progressbar.ProgressBar(max_value=n_instances) as p: - for counter in range(n_instances): - slice_f = get_df_theory_simple(delta_m, frequencies, flat=True, d0=d0) - - t0 = time.time() - distances_fft, probs_fft, _ = get_probability_bayes( - slice_f, - frequencies, - mic_idx=MIC_IDX, - distance_range=[min(distances_grid), max(distances_grid)], - azimuth_deg=AZIMUTH_DEG, - ) - d_estimate = distances_fft[np.argmax(probs_fft)] - times["fft"].append(time.time() - t0) - - t0 = time.time() - probs_cost = get_probability_cost( - slice_f, - frequencies, - distances_grid, - mic_idx=MIC_IDX, - azimuth_deg=AZIMUTH_DEG, - ) - d_estimate = distances_grid[np.argmax(probs_cost)] - times["cost"].append(time.time() - t0) - - p.update(counter) + p = progressbar.ProgressBar(maxval=n_instances) + for counter in range(n_instances): + slice_f = get_df_theory_simple(delta_m, frequencies, flat=True, d0=d0) + + t0 = time.time() + distances_fft, probs_fft, _ = get_probability_bayes( + slice_f, + frequencies, + mic_idx=MIC_IDX, + distance_range=[min(distances_grid), max(distances_grid)], + azimuth_deg=AZIMUTH_DEG, + ) + d_estimate = distances_fft[np.argmax(probs_fft)] + times["fft"].append(time.time() - t0) + + t0 = time.time() + probs_cost = get_probability_cost( + slice_f, + frequencies, + distances_grid, + mic_idx=MIC_IDX, + azimuth_deg=AZIMUTH_DEG, + ) + d_estimate = distances_grid[np.argmax(probs_cost)] + times["cost"].append(time.time() - t0) + + p.update(counter) return times @@ -284,8 +286,7 @@ def compare_timing(n_instances): sigmas_y, n_instances, ) - pd.to_pickle(results_df, fname) - print("saved as", fname) + save_pickle(results_df, fname) frequencies = [1000, 3000, 10000] # np.linspace(1000, 5000, 3) n_instances = 10 @@ -303,8 +304,7 @@ def compare_timing(n_instances): sigmas_y, n_instances, ) - pd.to_pickle(results_df, fname) - # print("saved as", fname) + save_pickle(results_df, fname) fname = "results/simulation/angle_amplitude_noise_new.pkl" sigmas_relative_cm = [0] # np.arange(10, step=2) @@ -319,8 +319,7 @@ def compare_timing(n_instances): sigmas_y, n_instances, ) - pd.to_pickle(results_df, fname) - print("saved as", fname) + save_pickle(results_df, fname) fname = "results/simulation/angle_joint_noise_new.pkl" frequencies = [3000] @@ -336,10 +335,8 @@ def compare_timing(n_instances): sigmas_y, n_instances, ) - pd.to_pickle(results_df, fname) - print("saved as", fname) + save_pickle(results_df, fname) -if 0: # __name__ == "__main__": ######### frequency slice study ### amplitude noise study n_instances = 10 @@ -356,8 +353,7 @@ def compare_timing(n_instances): results_df = simulate_frequency_slice( distances_cm, frequencies, sigmas_delta_cm, sigmas_f, sigmas_y, n_instances, ) - pd.to_pickle(results_df, fname) - print("saved as", fname) + save_pickle(results_df, fname) ### delta noise study fname = "results/simulation/delta_noise_new.pkl" @@ -368,8 +364,7 @@ def compare_timing(n_instances): results_df = simulate_frequency_slice( distances_cm, frequencies, sigmas_delta_cm, sigmas_f, sigmas_y, n_instances, ) - pd.to_pickle(results_df, fname) - print("saved as", fname) + save_pickle(results_df, fname) ### frequency noise study fname = "results/simulation/frequency_noise_new.pkl" @@ -380,8 +375,7 @@ def compare_timing(n_instances): results_df = simulate_frequency_slice( distances_cm, frequencies, sigmas_delta_cm, sigmas_f, sigmas_y, n_instances, ) - pd.to_pickle(results_df, fname) - print("saved as", fname) + save_pickle(results_df, fname) fname = "results/simulation/joint_noise_new.pkl" sigmas_f = [0] @@ -391,9 +385,4 @@ def compare_timing(n_instances): results_df = simulate_frequency_slice( distances_cm, frequencies, sigmas_delta_cm, sigmas_f, sigmas_y, n_instances, ) - pd.to_pickle(results_df, fname) - print("saved as", fname) - - times = compare_timing(n_instances) - for method, time_list in times.items(): - print(f"average time for {method}: {np.mean(time_list)/n_instances:.3e}s") + save_pickle(results_df, fname) diff --git a/python/generate_stepper_results.py b/python/generate_stepper_results.py index 4bbef503..a65ec0c4 100644 --- a/python/generate_stepper_results.py +++ b/python/generate_stepper_results.py @@ -81,10 +81,9 @@ def data_collector_from_df(df_all, exp_name, mic_type, motors, bin_selection="") df_all = pd.read_pickle(fname) except Exception as e: print("Error: run wall_analysis.py to parse experiments.") + print(e) sys.exit() - print("done", df_all) - for mic_type, motors, bin_selection in itertools.product( mic_types, motors_types, bin_selection_types ): diff --git a/python/plot_helpers.py b/python/plot_helpers.py index fb918639..9542e27b 100644 --- a/python/plot_helpers.py +++ b/python/plot_helpers.py @@ -1,3 +1,4 @@ + color = { "theoretical": "black", "calibrated": "C2", @@ -9,13 +10,17 @@ "histogram 1": ":", "histogram 3": "-.", "histogram 5": "--", + "histogram": (0, (1, 1)), "particle": "-", + "split particle": (0, (3, 1, 1, 1, 1, 1)), "single": (0, (1, 4)), "": "-" # random # "": (0, (5, 7)), # random } labels = {f"histogram {i}": f"hist. $N_w={i}$" for i in [1, 3, 5]} labels["particle"] = "particle" +labels["histogram"] = "histogram" +labels["split particle"] = "split particle" units = { "bayes angle": "deg", "bayes distance": "cm", diff --git a/python/prop_signal.wav b/python/prop_signal.wav new file mode 100644 index 00000000..4296b799 Binary files /dev/null and b/python/prop_signal.wav differ diff --git a/python/requirements.txt b/python/requirements.txt index 5348b5f4..34ab94c0 100644 --- a/python/requirements.txt +++ b/python/requirements.txt @@ -3,5 +3,7 @@ pandas matplotlib scipy>=1.7.3 numpy>=1.22.1 -pyroomacoustics>=0.6.0 +#pyroomacoustics>=0.6.0 optional unless you want to use pyroomacoustics for simulation pyaml +progressbar +filterpy diff --git a/python/test/helpers.py b/python/test/helpers.py index 78f31307..fb459718 100644 --- a/python/test/helpers.py +++ b/python/test/helpers.py @@ -14,11 +14,27 @@ ANGLE_GLOB = 90 DELTA_GRID = np.arange(100) +ANGLE_GRID = np.arange(360) + +DIFF_STD = 3.0 +ANGLE_STD = 10 + +MIC_IDX = [0, 1] + + +def get_diff_dict(distance, angle, prob_method): + diff_dict = {} + for mic_idx in MIC_IDX: + delta_grid, probs = get_delta_distribution( + distance, angle, mic_idx, prob_method + ) + diff_dict[mic_idx] = (delta_grid, probs) + return diff_dict def measure_wall(pose): - """ - simplified function for measuring the wall distance and angle in + """ + simplified function for measuring the wall distance and angle in local coordinates. Works for this geometry only. """ distance = DISTANCE_GLOB - pose[1] @@ -26,8 +42,15 @@ def measure_wall(pose): angle %= 360 return distance, angle +def get_angle_distribution(angle, prob_method="delta", scale=ANGLE_STD): + if prob_method == "delta": + probs = np.zeros(len(ANGLE_GRID)) + probs[np.argmin(np.abs(ANGLE_GRID - angle))] = 1.0 + elif prob_method == "normal": + probs = norm.pdf(ANGLE_GRID, loc=angle, scale=ANGLE_STD) + return ANGLE_GRID, probs -def get_delta_distribution(distance, angle, mic_idx, prob_method="delta", scale=10): +def get_delta_distribution(distance, angle, mic_idx, prob_method="delta", scale=DIFF_STD): delta = ( get_deltas_from_global( azimuth_deg=angle, distances_cm=distance, mic_idx=mic_idx diff --git a/python/test/test_estimators.py b/python/test/test_estimators.py index aab3ab3c..3ae136d4 100644 --- a/python/test/test_estimators.py +++ b/python/test/test_estimators.py @@ -7,34 +7,36 @@ import sys, os import numpy as np +import matplotlib.pylab as plt sys.path.append(os.path.abspath(os.path.join(os.getcwd(), "../"))) -from utils.moving_estimators import MovingEstimator, get_estimate +from utils.moving_estimators import MovingEstimator +from utils.histogram_estimators import HistogramEstimator +from utils.base_estimator import get_estimate -from helpers import measure_wall, get_delta_distribution - -MIC_IDX = [0, 1] +from helpers import measure_wall, get_diff_dict, DISTANCE_GLOB VERBOSE = False - ESTIMATION_METHOD = "max" # only max works for these examples. +def test_example_delta(method="moving"): + print("==== testing example with delta ====") + do_test_example("delta", n_window=2, method=estimator) + do_test_example("delta", n_window=3, method=estimator) + do_test_example("delta", n_window=4, method=estimator) -def test_rotation_delta(): - print("==== testing rotation with delta ====") - do_test_rotation("delta", n_window=2) - do_test_rotation("delta", n_window=3) - do_test_rotation("delta", n_window=4) - -def test_rotation_normal(): - print("==== testing rotation with normal ====") - do_test_rotation("normal", n_window=2) - do_test_rotation("normal", n_window=3) - do_test_rotation("normal", n_window=4) +def test_example_normal(method="moving"): + print("==== testing example with normal ====") + if method == "moving": + do_test_example("normal", n_window=2, method=estimator) + do_test_example("normal", n_window=3, method=estimator) + do_test_example("normal", n_window=4, method=estimator) + else: + do_test_example("normal", n_window=2, method=estimator) -def test_simplify_angles(): +def test_simplify_angles(method="moving"): print("==== testing simplified angles ====") for movement_dir_deg in [90, 270]: do_test_linear( @@ -58,44 +60,156 @@ def test_simplify_angles(): ) -def test_linear_many_directions(): +def test_linear_many_directions(method="moving"): print("==== testing many directions ====") - for movement_dir_deg in np.arange(360, step=20): - do_test_linear("normal", n_window=3, movement_dir_deg=movement_dir_deg) - do_test_linear("delta", n_window=3, movement_dir_deg=movement_dir_deg) + for movement_dir_deg in np.arange(360, step=45): + do_test_linear( + "normal", n_window=3, movement_dir_deg=movement_dir_deg, method=estimator + ) + do_test_linear( + "delta", n_window=3, movement_dir_deg=movement_dir_deg, method=estimator + ) -def test_linear_delta(): +def test_linear_delta(method="moving"): print("==== testing linear with delta ====") - do_test_linear("delta", n_window=2) - do_test_linear("delta", n_window=3) - do_test_linear("delta", n_window=4) + do_test_linear("delta", n_window=2, method=estimator) + do_test_linear("delta", n_window=3, method=estimator) + do_test_linear("delta", n_window=4, method=estimator) -def test_linear_normal(): +def test_linear_normal(method="moving"): print("==== testing linear with normal ====") - do_test_linear("normal", n_window=2) - do_test_linear("normal", n_window=3) - do_test_linear("normal", n_window=4) + if method == "moving": + do_test_linear("normal", n_window=2, method=estimator) + do_test_linear("normal", n_window=3, method=estimator) + do_test_linear("normal", n_window=4, method=estimator) + else: + do_test_linear("normal", n_window=2, method=estimator) + +def test_rotation_normal(method="moving"): + print("==== testing rotation with normal ====") + if method == "moving": + do_test_rotation("normal", n_window=2, method=estimator) + do_test_rotation("normal", n_window=3, method=estimator) + do_test_rotation("normal", n_window=4, method=estimator) + else: + do_test_rotation("normal", n_window=2, method=estimator) + + +def test_histogram_predict(prob_method="normal", n_window=2, verbose=False): + angles_deg = np.arange(360, step=30) + distances_cm = np.arange(50, step=10) + estimator = HistogramEstimator( + angles_deg=angles_deg, distances_cm=distances_cm + ) + poses = [[0, 10, -90], [10, 20, -180], [20, 10, 90], [10, 0, 0]] + for i, pose in enumerate(poses): + distance, angle = measure_wall(pose) + diff_dict = get_diff_dict(distance, angle, prob_method) + estimator.add_distributions(diff_dict, pose[:2], pose[2]) + if verbose: + print(f"added {distance, angle} at pose {i}") + + prior1 = estimator.predict_slow(verbose=verbose) + prior2 = estimator.predict(verbose=verbose) + try: + np.testing.assert_allclose(prior1, prior2) + except: + plt.figure() + plt.semilogy(prior1, label="prior slow") + plt.semilogy(prior2, label="prior fast") + plt.legend() + plt.show() + raise + + +def test_histogram_distributions(verbose=False, movement_dir_deg=90): + yaw_deg = movement_dir_deg - 90 + prob_method = "normal" + + angles_deg = np.array([90 + yaw_deg]) + step_distance_cm = 1 + distances_cm = np.arange(35, step=step_distance_cm) + estimator = HistogramEstimator(angles_deg=angles_deg, distances_cm=distances_cm) + + estimator = HistogramEstimator(angles_deg=angles_deg, distances_cm=distances_cm) + + poses = [ + [ + step * np.cos(movement_dir_deg / 180 * np.pi), + step * np.sin(movement_dir_deg / 180 * np.pi), + yaw_deg, + ] + for step in np.arange(26, step=5) # 0, 5, 15, 20, 25 + ] + for i, pose in enumerate(poses): + distance, angle = measure_wall(pose) + assert angle == angles_deg[0] + diff_dict = get_diff_dict(distance, angle, prob_method) + estimator.add_distributions(diff_dict, pose[:2], pose[2]) + if verbose: + print(f"added {distance:.2f}, {angle:.0f} at pose {i}") + + if i < 1: + continue + + # plt.figure() + # plt.plot(estimator.distances_cm, estimator.prior, label="prior before predict") + # plt.plot(estimator.distances_cm, estimator.posterior, label="posterior before predict") + ( + distances, + prob_distances, + angles, + prob_angles, + ) = estimator.get_distributions(verbose=verbose, simplify_angles=False) + # plt.plot(estimator.distances_cm, estimator.prior, label="prior after predict") + # plt.plot(estimator.distances_cm, estimator.posterior, label="posterior after update") + # plt.legend() + # plt.show() + + distance_estimate, __ = estimator.get_distance_estimate( + prob_distances, method=ESTIMATION_METHOD + ) + angle_estimate, __ = estimator.get_angle_estimate( + prob_angles, method=ESTIMATION_METHOD + ) + try: + assert ( + abs(distance_estimate - distance) <= 2 * step_distance_cm + ), f"distance at position {i}: {distance_estimate} != true {distance}. Correct angle: {angle}" + except Exception as e: + print(e) def do_test_linear( - prob_method, n_window, movement_dir_deg=70, simplify_angles=False, verbose=VERBOSE + prob_method, + n_window, + movement_dir_deg=90, + simplify_angles=False, + verbose=VERBOSE, + method="moving", ): - """ Test on rotating toy example """ - print(f"testing {prob_method}, {n_window}, {movement_dir_deg}, {simplify_angles}") + """ Test on linear movement towards the wall. """ + if verbose: + print(f"testing {prob_method}, {n_window}, {movement_dir_deg}, {simplify_angles}") yaw_deg = movement_dir_deg - 90 step_angle_deg = 10 step_distance_cm = 0.5 # angles_deg = [ANGLE_GLOB - yaw_deg] # ground truth only angles_deg = np.arange(360, step=step_angle_deg) - distances_cm = np.arange(0, 60, step=step_distance_cm) - moving_estimator = MovingEstimator( - n_window=n_window, angles_deg=angles_deg, distances_cm=distances_cm - ) - moving_estimator.ANGLE_WINDOW_DEG = 40 - moving_estimator.ANGLE_RESOLUTION_DEG = 10 + distances_cm = np.arange(0, 50, step=step_distance_cm) + if method == "moving": + estimator = MovingEstimator( + n_window=n_window, angles_deg=angles_deg, distances_cm=distances_cm + ) + estimator.ANGLE_WINDOW_DEG = 40 + estimator.ANGLE_RESOLUTION_DEG = 10 + elif method == "histogram": + estimator = HistogramEstimator( + angles_deg=angles_deg, distances_cm=distances_cm + ) poses = [ [ @@ -103,19 +217,14 @@ def do_test_linear( step * np.sin(movement_dir_deg / 180 * np.pi), yaw_deg, ] - for step in np.linspace(0, 19, 3) + for step in np.arange(20, step=5) # 0, 5, 15 ] for i, pose in enumerate(poses): distance, angle = measure_wall(pose) - - diff_dict = {} - for mic_idx in MIC_IDX: - delta_grid, probs = get_delta_distribution( - distance, angle, mic_idx, prob_method - ) - diff_dict[mic_idx] = (delta_grid, probs) - moving_estimator.add_distributions(diff_dict, pose[:2], pose[2]) - # print(f"added {distance:.2f}, {angle:.0f} at pose {i}") + diff_dict = get_diff_dict(distance, angle, prob_method) + estimator.add_distributions(diff_dict, pose[:2], pose[2]) + if verbose: + print(f"added {distance:.2f}, {angle:.0f} at pose {i}") if i < n_window - 1: continue @@ -125,19 +234,23 @@ def do_test_linear( prob_distances, angles, prob_angles, - ) = moving_estimator.get_distributions( + ) = estimator.get_distributions( verbose=verbose, simplify_angles=simplify_angles ) - distance_estimate, __ = moving_estimator.get_distance_estimate( + distance_estimate, __ = estimator.get_distance_estimate( prob_distances, method=ESTIMATION_METHOD ) + if distance_estimate is None: + print(f"Warning: for pose {i} and movement angle {movement_dir_deg}, distance estimate is None") + continue + if simplify_angles: angle_estimate, __ = get_estimate( angles, prob_angles, method=ESTIMATION_METHOD ) else: - angle_estimate, __ = moving_estimator.get_angle_estimate( + angle_estimate, __ = estimator.get_angle_estimate( prob_angles, method=ESTIMATION_METHOD ) @@ -156,11 +269,9 @@ def do_test_linear( prob_distances, angles, prob_angles, - ) = moving_estimator.get_distributions( - verbose=True, simplify_angles=simplify_angles, plot_angles=[angle] + ) = estimator.get_distributions( + verbose=False, simplify_angles=simplify_angles, plot_angles=[angle] ) - import matplotlib.pylab as plt - fig, axs = plt.subplots(2) axs[0].plot(angles, prob_angles) axs[0].axvline(angle_estimate, color="r") @@ -173,34 +284,47 @@ def do_test_linear( axs[1].set_xlabel("distance [cm]") axs[1].grid(which="both") plt.show(block=True) - raise e -def do_test_rotation(prob_method, n_window): - """ Test on rotating toy example """ +def do_test_rotation( + prob_method, + n_window, + static_distance_cm=30, + verbose=VERBOSE, + method="moving", +): + """ Test on linear movement towards the wall. """ + if verbose: + print(f"testing {prob_method}, {n_window}, {static_distance_cm}") + step_angle_deg = 10 - step_distance_cm = 10 angles_deg = np.arange(360, step=step_angle_deg) - distances_cm = np.arange(100, step=step_distance_cm) - moving_estimator = MovingEstimator( - n_window=n_window, angles_deg=angles_deg, distances_cm=distances_cm - ) - - # toy example: the robot does a diamond-shaped movement, - # and rotate by -90 degrees at each position. - # the wall is located at 30cm north from the origin, horizontal. + distances_cm = np.array([static_distance_cm]) + if method == "moving": + estimator = MovingEstimator( + n_window=n_window, angles_deg=angles_deg, distances_cm=distances_cm + ) + estimator.ANGLE_WINDOW_DEG = 40 + estimator.ANGLE_RESOLUTION_DEG = 10 + elif method == "histogram": + estimator = HistogramEstimator( + angles_deg=angles_deg, distances_cm=distances_cm + ) - poses = [[0, 10, -90], [10, 20, -180], [20, 10, 90], [10, 0, 0]] + poses = [ + [ + 0, + DISTANCE_GLOB-static_distance_cm, + yaw_deg, + ] + for yaw_deg in np.arange(360, step=step_angle_deg) # 0, 5, 15 + ] for i, pose in enumerate(poses): distance, angle = measure_wall(pose) - diff_dict = {} - for mic_idx in MIC_IDX: - delta_grid, probs = get_delta_distribution( - distance, angle, mic_idx, prob_method - ) - diff_dict[mic_idx] = (delta_grid, probs) - moving_estimator.add_distributions(diff_dict, pose[:2], pose[2]) - print(f"added {distance, angle} at pose {i}") + diff_dict = get_diff_dict(distance, angle, prob_method) + estimator.add_distributions(diff_dict, pose[:2], pose[2]) + if verbose: + print(f"added {distance:.2f}, {angle:.0f} at pose {i}") if i < n_window - 1: continue @@ -210,31 +334,115 @@ def do_test_rotation(prob_method, n_window): prob_distances, angles, prob_angles, - ) = moving_estimator.get_distributions(verbose=VERBOSE, simplify_angles=False) + ) = estimator.get_distributions( + verbose=verbose, simplify_angles=False + ) - distance_estimate, __ = moving_estimator.get_distance_estimate( + distance_estimate, __ = estimator.get_distance_estimate( prob_distances, method=ESTIMATION_METHOD ) - angle_estimate, __ = moving_estimator.get_angle_estimate( + angle_estimate, __ = estimator.get_angle_estimate( prob_angles, method=ESTIMATION_METHOD ) + assert ( - abs(distance_estimate - distance) <= 2 * step_distance_cm - ), f"distance at position {i}: {distance_estimate} != true {distance}" + abs(distance_estimate - distance) == 0 + ), f"distance at position {i}: {distance_estimate} != true {distance}. Correct angle: {angle}" assert ( abs(angle_estimate - angle) <= 2 * step_angle_deg - ), f"angle at position {i}: {angle_estimate} != true {angle}" + ), f"angle at position {i}: {angle_estimate} != true {angle}. Correct distance: {distance}" + + +def do_test_example(prob_method, n_window, method="moving", verbose=VERBOSE): + """Test on rotating toy example""" + step_angle_deg = 30 + step_distance_cm = 5 + angles_deg = np.arange(0, 360, step=step_angle_deg) + distances_cm = np.arange(70, step=step_distance_cm) + if method == "moving": + estimator = MovingEstimator( + n_window=n_window, angles_deg=angles_deg, distances_cm=distances_cm + ) + elif method == "histogram": + estimator = HistogramEstimator( + angles_deg=angles_deg, distances_cm=distances_cm + ) + # toy example: the robot does a diamond-shaped movement, + # and rotate by -90 degrees at each position. + # the wall is located at 30cm north from the origin, horizontal. + # see test_estimator.png for drawing. -if __name__ == "__main__": - # from utils.geometry import Context - # print(Context.get_platform_setup().mics) + poses = [[0, 10, -90], [10, 20, -180], [20, 10, 90], [10, 0, 0]] + #poses = [[10, 20, -180], [20, 10, 90]] # backward + #poses = [[0, 10, -90], [10, 20, -180]] # forward + for i, pose in enumerate(poses): + distance, angle = measure_wall(pose) + diff_dict = get_diff_dict(distance, angle, prob_method) + if verbose: + print(f"adding {distance, angle} at pose {i}") + estimator.add_distributions(diff_dict, pose[:2], pose[2]) - test_linear_delta() - test_linear_normal() - test_linear_many_directions() - test_simplify_angles() + if i < n_window - 1: + continue + + if verbose: + print(f"doing inference...") + ( + distances, + prob_distances, + angles, + prob_angles, + ) = estimator.get_distributions(verbose=VERBOSE, simplify_angles=False) - test_rotation_delta() - test_rotation_normal() + distance_estimate, __ = estimator.get_distance_estimate( + prob_distances, method=ESTIMATION_METHOD + ) + angle_estimate, __ = estimator.get_angle_estimate( + prob_angles, method=ESTIMATION_METHOD + ) + try: + assert ( + abs(distance_estimate - distance) <= 2 * step_distance_cm + ), f"distance at position {i}: {distance_estimate} != true {distance}" + assert ( + abs(angle_estimate - angle) <= 2 * step_angle_deg + ), f"angle at position {i}: {angle_estimate} != true {angle}" + except Exception as e: + print(e) + + plt.figure() + plt.plot(angles, prob_angles, color="C0", label="posterior") + if method == "histogram": + prior = estimator.get_marginal(posterior=False, distance_cm=distance_estimate) + posterior = estimator.get_marginal(posterior=True, distance_cm=distance_estimate) + plt.plot(angles, prior, color="C1", label="prior marginal") + plt.plot(angles, posterior, color="C2", label="posterior marginal") + plt.axvline(angle_estimate, color='red', label="estimated") + plt.axvline(angle, color='green', label="correct") + plt.title(f"pose {i}") + plt.legend() + plt.show() + +if __name__ == "__main__": + import sys + + print("---------------- Testing histogram filter -----------------") + estimator = "histogram" + test_histogram_predict() + test_histogram_distributions() + test_linear_normal(estimator) + test_rotation_normal(estimator) + test_example_normal(estimator) + + print("---------------- Testing moving filter -----------------") + estimator = "moving" + test_linear_delta(estimator) + test_linear_normal(estimator) + test_rotation_normal(estimator) + test_linear_many_directions(estimator) + test_simplify_angles(estimator) + + test_example_normal(estimator) + test_example_delta(estimator) print("done") diff --git a/python/test/test_particle.py b/python/test/test_particle.py index d6fdc334..155a61d0 100644 --- a/python/test/test_particle.py +++ b/python/test/test_particle.py @@ -11,60 +11,51 @@ sys.path.append(os.path.abspath(os.path.join(os.getcwd(), "../"))) from utils.particle_estimators import ParticleEstimator -from helpers import get_delta_distribution, measure_wall, DISTANCE_GLOB, ANGLE_GLOB +from helpers import measure_wall, get_diff_dict, get_angle_distribution MIC_IDX = [0, 1] SCALE = 500 PREDICT_UNIFORM = False +BEAMFORM = True -def test_particle_filter(): - distances_cm = np.arange(40) - angles_deg = np.arange(360, step=10) +def test_particle_filter(): + distances_cm = np.arange(50) + angles_deg = np.arange(360) n_particles = 100 - particle_filter = ParticleEstimator( - n_particles=n_particles, - global_=False, - predict_uniform=True, - distances_cm=distances_cm, - angles_deg=angles_deg, - ) - do_test(particle_filter, title="local, uniform", global_=False) - particle_filter = ParticleEstimator( - n_particles=n_particles, - global_=False, - predict_uniform=False, - distances_cm=distances_cm, - angles_deg=angles_deg, - ) - do_test(particle_filter, title="local", global_=False) + #particle_filter = ParticleEstimator( + # n_particles=n_particles, + # predict_uniform=True, + # distances_cm=distances_cm, + # angles_deg=angles_deg, + #) + #do_test(particle_filter, title="predict uniform") particle_filter = ParticleEstimator( n_particles=n_particles, - global_=True, - predict_uniform=True, + predict_uniform=False, distances_cm=distances_cm, angles_deg=angles_deg, ) - do_test(particle_filter, title="global", global_=True) + do_test(particle_filter, title="predict non-uniform") plt.show() -def do_test(particle_filter, title="", global_=True): +def do_test(particle_filter, title=""): def plot(x, col): axs[0].scatter( [x] * particle_filter.n_particles, - particle_filter.particles[:, 0], + particle_filter.states[:, 0], color=f"C{col}", s=particle_filter.weights * SCALE, alpha=0.5, ) axs[1].scatter( [x] * particle_filter.n_particles, - particle_filter.particles[:, 1], + particle_filter.states[:, 1], color=f"C{col}", s=particle_filter.weights * SCALE, alpha=0.5, @@ -72,10 +63,10 @@ def plot(x, col): def plot_final(col): axs[2].scatter( - particle_filter.particles[:, 0] - * np.cos(particle_filter.particles[:, 1] / 180 * np.pi), - particle_filter.particles[:, 0] - * np.sin(particle_filter.particles[:, 1] / 180 * np.pi), + particle_filter.states[:, 1] + * np.cos(particle_filter.states[:, 0] / 180 * np.pi), + particle_filter.states[:, 0] + * np.sin(particle_filter.states[:, 0] / 180 * np.pi), color=f"C{col}", s=particle_filter.weights * SCALE, alpha=0.5, @@ -83,43 +74,39 @@ def plot_final(col): def plot_estimate(x, col): distance_estimate, angle_estimate = particle_filter.estimate()[0] - axs[0].scatter(x, distance_estimate, color=f"k", marker="x", label="estimate") - axs[1].scatter(x, angle_estimate, color=f"k", marker="x", label="estimate") + axs[0].scatter(x, angle_estimate, color=f"k", marker="x", label="estimate") + axs[1].scatter(x, distance_estimate, color=f"k", marker="x", label="estimate") movement_dir_deg = 90 yaw_deg = movement_dir_deg - 90 + + rots = np.arange(0, 20) + steps = np.arange(0, 20) poses = [ [ step * np.cos(movement_dir_deg / 180 * np.pi), step * np.sin(movement_dir_deg / 180 * np.pi), - yaw_deg, + rot, ] - for step in np.arange(20, step=3.0) + for rot, step in zip(rots, steps) ] - # particle_filter_global = ParticleEstimator(n_particles=n_particles, global_=True, predict_uniform=PREDICT_UNIFORM) - fig, axs = plt.subplots(1, 3) fig.suptitle(title) plot(0, 0) - plot_final(0) # fig_pos, axs_pos = plt.subplots(len(poses), 2) # fig_pos.suptitle(f"{title} over time") for i, pose in enumerate(poses): distance, angle = measure_wall(pose) print("local distance and angle", distance, angle) - print("pose:", pose) - - diff_dict = {} - for mic_idx in MIC_IDX: - delta_grid, probs = get_delta_distribution( - distance, angle, mic_idx, prob_method="normal" - ) - diff_dict[mic_idx] = (delta_grid, probs) + diff_dict = get_diff_dict(distance, angle, prob_method="normal") particle_filter.add_distributions(diff_dict, pose[:2], pose[2]) + angles, probs = get_angle_distribution(angle, prob_method="normal") + particle_filter.add_angle_distribution(angles, probs) + particle_filter.predict() plot(i + 1, i + 1) particle_filter.update() @@ -128,35 +115,20 @@ def plot_estimate(x, col): plot(i + 1.6, i + 1) plot_estimate(i + 1.5, i + 1) - if (i % 5 == 0) or (i == len(poses) - 1): - plot_final(i + 1) - - if not global_: - axs[0].scatter( - i + 1.5, distance, color=f"k", label="ground truth", marker="+" - ) - axs[1].scatter(i + 1.5, angle, color=f"k", label="ground truth", marker="+") - - # axs_pos[i, 0].set_xlabel("distance [cm]") - # axs_pos[i, 1].set_xlabel("angle [deg]") - # for method in ["histogram", "gaussian"]: - # ( - # distances_cm, - # probs_dist, - # angles_deg, - # probs_angles, - # ) = particle_filter.get_distributions(method=method) - # axs_pos[i, 0].plot(distances_cm, probs_dist, label=method) - # axs_pos[i, 1].plot(angles_deg, probs_angles, label=method) - # axs_pos[i, 1].set_xticks(angles_deg) - # axs_pos[i, 1].set_xticklabels(angles_deg) - - if global_: - axs[0].axhline(DISTANCE_GLOB, color=f"k", label="ground truth") - axs[1].axhline(ANGLE_GLOB, color=f"k", label="ground truth") - axs[1].set_ylim(0, 360) - - axs[1].set_ylim(0, 360) + axs[1].scatter( + i + 1.5, distance, color=f"k", label="ground truth", marker="+" + ) + axs[0].scatter(i + 1.5, angle, color=f"k", label="ground truth", marker="+") + plot_final(0) + + axs[0].set_title("angle particle evolution") + axs[1].set_title("distance particle evolution") + axs[2].set_title("final plane estimate") + axs[2].set_xlabel("x [cm]") + axs[2].set_ylabel("y [cm]") + + axs[0].set_ylim(0, 360) + axs[1].set_ylim(0, 50) if __name__ == "__main__": diff --git a/python/utils/base_estimator.py b/python/utils/base_estimator.py new file mode 100644 index 00000000..0d05f3fc --- /dev/null +++ b/python/utils/base_estimator.py @@ -0,0 +1,253 @@ +#! /usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +base_estimator.py: Provides abstract base class for hisogram and particle filter classes. +""" +import numpy as np +import scipy.interpolate + +from .constants import PLATFORM +from .geometry import Context + +import warnings + + +def from_0_to_360(angle): + if np.ndim(angle) > 0: + angle = np.mod(angle, 360) + angle[angle < 0] += 360 + else: + angle %= 360 + angle = angle + 360 if angle < 0 else angle + return angle + + +def from_0_to_180(angle): + if np.ndim(angle) > 0: + angle = np.mod(angle, 360) # between + angle[angle >= 180] = 360 - angle[angle >= 180] + else: + angle %= 360 + angle = 360 - angle if angle > 180 else angle + return angle + + +def get_std_sample(values, probs, means, unbiased=True): + if np.isclose(np.nansum(probs), 1) or ( + np.nansum(probs) < 1 + ): # for distributions (might be unnormalized) + norm = 1.0 + else: # for histograms + norm = ( + (np.nansum(probs) - 1) if unbiased else np.nansum(probs) + ) # for histograms + if not norm: + return None if np.ndim(means) == 0 else np.full(len(means), None) + + if np.ndim(means) > 0: + coeffs = np.multiply(probs[:, None], (values[:, None] - means[None, :]) ** 2) + var = np.nansum(coeffs, axis=0) / norm + else: + var = np.nansum(np.multiply(probs, (values - means) ** 2), axis=0) / norm + return np.sqrt(var) + + +def get_std_of_peaks(values, probs, peaks): + widths, *__ = scipy.signal.peak_widths(probs, peaks) + fwhm = widths * (values[1] - values[0]) # assumes uniform values + return fwhm / 2 / np.sqrt(2 * np.log(2)) + + + + +def get_estimate(values, probs, method, unbiased=True): + # distribution with only one value, return that value. + if len(values) == 1: + return values[0], 0.0 + + if np.nansum(probs) == 0: + print("Warning: all zeros") + return None, None + + if method == "mean": + mean = np.nansum(np.multiply(probs, values)) / np.nansum(probs) + std = get_std_sample(values, probs, means=mean, unbiased=unbiased) + return mean, std + elif method == "max": + max_prob = np.nanmax(probs) + estimates = values[np.where(probs == max_prob)[0]] + stds = get_std_sample(values, probs, means=estimates, unbiased=unbiased) + elif method == "peak": + indices, __ = scipy.signal.find_peaks(probs) + if not len(indices): + return None, None + prob_estimates = probs[indices] + max_prob = np.max(prob_estimates) + indices = np.where(probs == max_prob)[0] + estimates = values[indices] + stds = get_std_of_peaks(values, probs, indices) + else: + raise ValueError(method) + + if len(estimates) == len(probs): + print(f"Warning: uniform distribution?") + return None, None + elif len(estimates) > 1: + pass + # print(f"Warning: ambiguous distribution, {len(estimates)} maxima") + elif not len(estimates): + # print(f"Warning: no valid estimates detected: {values}, {probs}") + return None, None + return estimates[0], stds[0] + + +def get_estimates(values, probs, method, sort=True, n_estimates=None): + """ + :param n_estimates: number of estimates (i.e. peaks) to return. None: return all. + :param sort: sort estimates by probability (most likely first). + """ + if n_estimates == 1: + mean, std = get_estimate(values, probs, method=method) + return [mean], [std] + if n_estimates is None or n_estimates > 1: + if method != "peak": + warnings.warn( + f"Using method peak instead of {method} for n_estimates={n_estimates}" + ) + + indices, properties = scipy.signal.find_peaks(probs, width=True) + if n_estimates and len(indices) < n_estimates: + print(f"Warning: found less peaks than requested: {len(indices)}") + + stds = get_std_of_peaks(values, probs, indices) + estimates = values[indices] + prob_estimates = probs[indices] + if sort: + sort_indices = prob_estimates.argsort()[::-1] + estimates = estimates[sort_indices] + prob_estimates = prob_estimates[sort_indices] + stds = stds[sort_indices] + + if n_estimates is None: + return estimates, stds + else: + return estimates[:n_estimates], stds[:n_estimates] + + +def get_normal_vector(angle_deg): + return np.r_[ + np.cos(angle_deg / 180 * np.pi), + np.sin(angle_deg / 180 * np.pi), + ] + +def get_normal_matrix(angles_vec): + """return N x 2""" + return np.c_[ + np.cos(angles_vec / 180 * np.pi), np.sin(angles_vec / 180 * np.pi) + ] + + +class BaseEstimator(object): + def __init__( + self, + n_window=2, + platform=PLATFORM, + ): + self.difference_p = {n: {} for n in range(n_window)} + self.angle_probs = {n: None for n in range(n_window)} + self.positions = np.full((n_window, 2), None) + self.rotations = np.full(n_window, None) + self.times = np.full(n_window, None) + + self.dim = 2 + self.reference = None + + self.counter = 0 # total counter + self.index = -1 # rolling index + self.n_window = n_window + self.filled = False + + self.context = Context.get_platform_setup(platform) + + def add_distributions( + self, + diff_dictionary, + position_cm=[0, 0], + rot_deg=0, + ): + """ + diff_dictionary: + { + mic_idx1: (diff1_cm, prob1), + mic_idx2: (diff2_cm, prob2), + ... + } + """ + if len(position_cm) > 2: + position_cm = position_cm[:2] + + self.index += 1 + if self.index == self.n_window - 1: + self.filled = True + elif self.index == self.n_window: # rolling window + self.index = 0 + + self.positions[self.index, :] = position_cm + self.rotations[self.index] = rot_deg + self.times[self.index] = self.counter + self.counter += 1 + + for mic_idx, (diff, prob) in diff_dictionary.items(): + if not any(diff > 1): + print("Warning: need to pass diff in centimeters") + self.difference_p[self.index][mic_idx] = scipy.interpolate.interp1d( + diff.astype(float), + prob.astype(float), + fill_value=0, + bounds_error=False, + kind="linear", + ) + + def add_angle_distribution(self, angles, probs): + self.angle_probs[self.index] = scipy.interpolate.interp1d( + angles, probs, fill_value=0, bounds_error=False, kind="linear" + ) + + def enough_measurements(self): + return self.filled + + def get_ordered_index(self): + """Get the list of indices that the current elements in the buffer correspond to. + For instance, if self.positions contains: [p7 p1 p2 p3 p4 p5 p6] + it returns ordered_index=[6, 0, 1, 2, 3, 4, 5], such that positions[ordered_index] + corresponds to the positions ordered chronologically: [p1, p2, p3, p4, p5, p6, p7], + with the oldest position first. + """ + if self.filled: + n_elements = self.n_window + else: + n_elements = self.index + 1 + ordered_index = np.roll(range(n_elements), -self.index - 1) + return ordered_index + + def get_forward_angle(self): + # TODO(FD) make this more general. + if self.positions[self.index][1] < 0: + return 270 + else: + return 90 + + def get_local_forward_angle(self): + current = self.index + if not self.filled and (self.index < 2): + # print("Warning: cannot simplify angles yet because buffer not filled.") + return 0 + # if self.positions is [p5, p6, p2, p3, p4] with current=2, return [p2, p3, p4, p5, p6] + ordered_positions = self.positions[self.get_ordered_index()] + + forward_dir_global = np.mean(np.diff(ordered_positions, axis=0), axis=0) + angle_global_deg = ( + 180 / np.pi * np.arctan2(forward_dir_global[1], forward_dir_global[0]) + ) + angle_local_deg = angle_global_deg - self.rotations[current] + return angle_local_deg diff --git a/python/utils/custom_argparser.py b/python/utils/custom_argparser.py index 0071f79a..37eaab7a 100644 --- a/python/utils/custom_argparser.py +++ b/python/utils/custom_argparser.py @@ -5,6 +5,12 @@ import argparse def exp_parser(description=""): + """ + Usage: + parser = exp_parser("DESCRIPTION OF SCRIPT") + parser. + + """ parser = argparse.ArgumentParser(description=description) parser.add_argument('--experiment_names', type=str, nargs='+', default = [], help='list of experiment names to process (folders inside "experiment_root".') diff --git a/python/utils/dataset_parameters.py b/python/utils/dataset_parameters.py index b0eea816..3b1178ae 100644 --- a/python/utils/dataset_parameters.py +++ b/python/utils/dataset_parameters.py @@ -280,7 +280,7 @@ }, "2021_10_12_hover": {"appendix": ["", "_7", "_11", "_12", "_40", "_41", "_42"],}, "2021_10_12_flying": { - "appendix": ["_new3", "_new4", "_new6", "_new7", "_new8", "_new10"] + "appendix": ["_new3", "_new6"] #["_new3", "_new4", "_new6", "_new7", "_new8", "_new10"] + [f"_{i}" for i in range(1, 7)] }, "2021_10_12_linear": {"appendix": ["", "_1", "_2", "_6", "_8", "_9", "_10"],}, diff --git a/python/utils/estimators.py b/python/utils/estimators.py index dfea0972..4edf22fb 100644 --- a/python/utils/estimators.py +++ b/python/utils/estimators.py @@ -21,11 +21,11 @@ def get_estimate(values, probs): def get_window(points, i, max_delta=10): """ - Get window around index i, splitting each interval in half. + Get window around index i, splitting each interval in half. At boundaries: use nearest interval's width for extrapolation. :param max_delta: maximum (double) width of window to use for integration. important when - we use coarse discretization grids. + we use coarse discretization grids. """ if i > 0: delta = min((points[i] - points[i - 1]) / 2, max_delta) @@ -64,6 +64,19 @@ def __init__(self, distances_cm=DISTANCES_CM, angles_deg=ANGLES_DEG): self.distances_cm = distances_cm self.angles_deg = angles_deg + def add_distributions(self, signals_f, frequencies, azimuth_deg, chosen_mics=None): + from .inference import get_probability_bayes + + if chosen_mics is None: + chosen_mics = range(signals_f.shape[1]) + + for i, mic_idx in enumerate(chosen_mics): + slice_f = np.abs(signals_f[:, i]) ** 2 + d_bayes, p_bayes, diff_cm = get_probability_bayes( + slice_f, frequencies, azimuth_deg=azimuth_deg + ) + self.add_distribution(diff_cm * 1e-2, p_bayes, mic_idx) + def add_distribution(self, path_differences_m, probabilities, mic_idx): if np.any(path_differences_m > 100): print("Warning: make sure path_differences_m is in meters!") @@ -75,7 +88,10 @@ def add_distribution(self, path_differences_m, probabilities, mic_idx): ) def get_distance_distribution( - self, chosen_mics=None, verbose=False, angle_deg=None, + self, + chosen_mics=None, + verbose=False, + angle_deg=None, ): if angle_deg is None: angles_deg = self.ANGLES_DEG_PRIOR diff --git a/python/utils/evaluate_data.py b/python/utils/evaluate_data.py index c5aabd06..8774b8a2 100644 --- a/python/utils/evaluate_data.py +++ b/python/utils/evaluate_data.py @@ -310,7 +310,7 @@ def get_positions_absolute(df_pos): """ if isinstance(df_pos, pd.DataFrame): if not len({"x", "y", "z", "yaw_deg"}.intersection(df_pos.columns.values)): - warnings.warn("no position information found") + #warnings.warn("no position information found") n_times = len(df_pos) return np.zeros((n_times, 4)) xs = df_pos.x.values diff --git a/python/utils/geometry.py b/python/utils/geometry.py index d5df1301..4a574e83 100644 --- a/python/utils/geometry.py +++ b/python/utils/geometry.py @@ -8,8 +8,6 @@ import numpy as np -from audio_stack.beam_former import rotate_mics - from .constants import PLATFORM DIM = 2 @@ -129,7 +127,7 @@ def get_angles(mic, source, delta_m, distance_m): class Context(object): - def __init__(self, dim=DIM, mics=None, source=None): + def __init__(self, dim=DIM, mics=None, source=None, name="unnamed context"): assert dim in (2, 3), dim self.dim = dim if mics is not None: @@ -138,6 +136,7 @@ def __init__(self, dim=DIM, mics=None, source=None): assert source.shape[0] == dim self.mics = mics self.source = source + self.name = name @staticmethod def get_platform_setup(platform=PLATFORM): @@ -154,7 +153,7 @@ def get_crazyflie_setup(dim=DIM): mics = np.array(MIC_POSITIONS)[:, :dim] source = np.array(BUZZER_POSITION)[0, :dim] - return Context(dim, mics, source) + return Context(dim, mics, source, name="crazyflie") @staticmethod def get_epuck_setup(dim=DIM): @@ -162,13 +161,13 @@ def get_epuck_setup(dim=DIM): mics = np.array(MIC_POSITIONS)[:, :dim] source = np.array(BUZZER_POSITION)[0, :dim] - return Context(dim, mics, source) + return Context(dim, mics, source, name="epuck") @staticmethod def get_random_setup(n_mics=4, dim=DIM): mics = np.random.uniform(low=-1.0, high=1.0, size=(n_mics, dim)) source = np.random.uniform(low=-1.0, high=1.0, size=(dim,)) - return Context(dim, mics, source) + return Context(dim, mics, source, name="random") @staticmethod def get_standard_setup(dim=DIM): @@ -180,7 +179,7 @@ def get_standard_setup(dim=DIM): elif dim == 3: mics = np.array([[X, X, Z], [-X, X, Z], [-X, -X, Z], [X, -X, Z]]) source = np.array([2 * X, 2 * X, Z]) - return Context(dim, mics, source) + return Context(dim, mics, source, name="standard") def get_source_image(self, normal): d = np.linalg.norm(normal) diff --git a/python/utils/histogram_estimators.py b/python/utils/histogram_estimators.py new file mode 100644 index 00000000..cac2132e --- /dev/null +++ b/python/utils/histogram_estimators.py @@ -0,0 +1,202 @@ +#! /usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +histogram_estimators.py: Provides histogram filter (HistogramEstimator) class. +""" + +import numpy as np +from scipy.stats import norm + +from .base_estimator import ( + BaseEstimator, + get_estimate, + get_normal_matrix, + get_normal_vector, + from_0_to_180, + from_0_to_360, +) + +SIGMA_D = 5.0 +SIGMA_A = 10.0 # in degrees + + +def normal_dist(argument, std): + return ( + 1 + / (std * np.sqrt(2 * np.pi)) + * np.exp(-argument.astype(float) ** 2 / (2 * std**2)) + ) + + +class HistogramEstimator(BaseEstimator): + DISTANCES_CM = np.arange(100, step=10) + ANGLES_DEG = np.arange(360, step=10) + ESTIMATION_METHOD = "max" + + def __init__( + self, + n_window=2, + platform="crazyflie", + angles_deg=ANGLES_DEG, + distances_cm=DISTANCES_CM, + ): + super().__init__( + n_window=n_window, + platform=platform, + ) + + self.distances_cm = distances_cm + self.angles_deg = angles_deg + self.initialize_states(angles_deg, distances_cm) + + def initialize_states(self, angles_deg, distances_cm): + aa, dd = np.meshgrid(angles_deg, distances_cm) + self.states = np.c_[aa.flatten(), dd.flatten()] # N x 2 + self.n_states = self.states.shape[0] + self.prior = np.full(self.n_states, 1 / self.n_states) + self.posterior = np.full(self.n_states, 1 / self.n_states) + + def add_distributions(self, *args, **kwargs): + super().add_distributions(*args, **kwargs) + # self.predict_slow() + self.predict() + self.update() + # dist = self.get_distance_estimate()[0] + # angle = self.get_angle_estimate()[0] + # print("predicted and updated. Current estimates:", dist, angle) + + def predict_slow(self, verbose=False): + if self.filled: + self.index = np.mod(self.index, self.n_window) + previous = np.mod(self.index - 1, self.n_window) + elif self.index == 0: + return self.prior + else: + self.index = self.index + previous = self.index - 1 + sigma_d = sigma_a = 1.0 + + u_t = self.positions[self.index] - self.positions[previous] + u_a = self.rotations[self.index] - self.rotations[previous] + + for k, (a_k, d_k) in enumerate(self.states): + sum_ = 0 + + for i, (a_i, d_i) in enumerate(self.states): + + # worked for backwards + # a_glob_i = a_i - self.rotations[previous] + # mu_d = d_i - u_t.dot(get_normal_vector(a_glob_i)) # mean of current distance estimate + + # mean of current distance and angle estimates + a_glob_i = a_i + self.rotations[previous] + mu_d = d_i - u_t.dot(get_normal_vector(a_glob_i)) + mu_a = a_i - u_a + + norm_d = normal_dist(d_k - mu_d, SIGMA_D) + norm_a = normal_dist(from_0_to_180(a_k - mu_a), SIGMA_A) + + sum_ += norm_d * norm_a * self.posterior[i] + + # if (d_i == 20) and (d_k == 10) and (a_i == 180) : + # print(f"global {a_glob_i:.0f}?=90, {a_i:.0f}?=180, {self.rotations[previous]:.0f}?=270") + # print(f"for current angle {a_k}: previous global {a_glob_i}?=90 {mu_d:.0f}?=10, {mu_a:.0f}?=270") + # if (d_i == 10) and (d_k == 20) and (a_i == 270) : + # print(f"for current angle {a_k}: previous global {a_glob_i}?=90 {mu_d:.0f}?=20, {mu_a:.0f}?=0") + self.prior[k] = sum_ + self.prior /= np.sum(self.prior) + return self.prior + + def predict(self, verbose=False): + + if (not self.filled) and (self.index == 0): + return self.prior + + # rolling window with two elements: + previous = 1 if self.index == 0 else 0 + + pos_delta = self.positions[self.index] - self.positions[previous] + rot_delta = self.rotations[self.index] - self.rotations[previous] + + # print("distance correction:", -normal_matrix(self.states[0, :]) @ u_t) + # a_glob_i = a_i + self.rotations[previous] + a_global = self.rotations[previous] + self.states[:, 0] + mu_a = from_0_to_360(self.states[:, 0] - rot_delta) + mu_d = self.states[:, 1] - get_normal_matrix(a_global) @ pos_delta + + arg_d_mat = self.states[:, 1][:, None] - mu_d[None, :] + norm_d = normal_dist(arg_d_mat.astype(float), SIGMA_D) + + arg_a_mat = from_0_to_180(self.states[:, 0][:, None] - mu_a[None, :]) + norm_a = normal_dist(arg_a_mat.astype(float), SIGMA_A) + + self.prior = np.sum(norm_d * norm_a * self.posterior[None, :], axis=1) + self.prior /= np.sum(self.prior) + return self.prior + + def update(self, verbose=False): + probs = np.empty(self.n_states) + for k, (a_local, d_local) in enumerate(self.states): + prob = 1.0 + for mic, diff_p in self.difference_p[self.index].items(): + delta_local_cm = self.context.get_delta(a_local, d_local, mic_idx=mic) + prob *= diff_p(delta_local_cm) # interpolate at delta + + if self.angle_probs[self.index] is not None: + prob *= self.angle_probs[self.index](a_local) # inteprolate at angle + + probs[k] = self.prior[k] * prob + self.posterior = probs / np.sum(probs) + + def get_marginal(self, distance_cm=None, angle_deg=None, posterior=True): + if posterior: + joint = self.posterior.reshape( + (len(self.distances_cm), len(self.angles_deg)) + ) + else: + joint = self.prior.reshape((len(self.distances_cm), len(self.angles_deg))) + if (distance_cm) and (angle_deg): + i_d = np.argmin(np.abs(self.distances_cm - distance_cm)) + a_d = np.argmin(np.abs(self.angles_deg - angle_deg)) + return joint[i_d, a_d] + elif distance_cm: + i_d = np.argmin(np.abs(self.distances_cm - distance_cm)) + return joint[i_d, :] + elif angle_deg: + a_d = np.argmin(np.abs(self.angles_deg - angle_deg)) + return joint[:, a_d] + + def get_distributions(self, simplify_angles=False, verbose=False, plot_angles=[]): + # self.predict(verbose=verbose) + # self.update(verbose=verbose) + # posterior = self.posterior.reshape((len(self.angles_deg), len(self.distances_cm))) + # prob_angle = np.sum(posterior, axis=1) + # prob_dist = np.sum(posterior, axis=0) + if simplify_angles: + raise NotImplementedError(simplify_angles) + if len(plot_angles): + raise NotImplementedError("plot angles") + + posterior = self.posterior.reshape( + (len(self.distances_cm), len(self.angles_deg)) + ) + prob_angle = np.sum(posterior, axis=0) + prob_dist = np.sum(posterior, axis=1) + return self.distances_cm, prob_dist, self.angles_deg, prob_angle + + posterior = self.posterior.reshape( + (len(self.angles_deg), len(self.distances_cm)) + ) + prob_angle = np.sum(posterior, axis=1) + prob_dist = np.sum(posterior, axis=0) + return self.distances_cm, prob_dist, self.angles_deg, prob_angle + + def get_distance_estimate(self, prob=None, method=ESTIMATION_METHOD): + if prob is None: + __, prob, *__ = self.get_distributions() + return get_estimate(self.distances_cm, prob, method=method) + + def get_angle_estimate(self, prob=None, method=ESTIMATION_METHOD): + if prob is None: + __, __, __, prob = self.get_distributions() + return get_estimate(self.angles_deg, prob, method=method) diff --git a/python/utils/inference.py b/python/utils/inference.py index 03182442..08584b3e 100644 --- a/python/utils/inference.py +++ b/python/utils/inference.py @@ -29,7 +29,7 @@ def get_uniform_grid(xvalues): - """ Fill too large spaces of xvalues with uniform sampling of the median spacing of xvalues. + """Fill too large spaces of xvalues with uniform sampling of the median spacing of xvalues. Example: [0, 2, 4, 10, 12, 14] becomes @@ -53,22 +53,47 @@ def get_uniform_grid(xvalues): def interpolate_parts(xvalues, values, step=None, verbose=False): - """ Smart interpolation scheme. + """Smart interpolation scheme. Interpolate at uniform grid, leaving out points that are further - from recorded data than two times the average or given spacing. + from recorded data than two times the average or given spacing. """ import scipy.interpolate if step is not None: - raise ValueError("Giving step is depcreated") + print("Warning: giving step is depcreated") + + #step = np.min(np.diff(xvalues)) + #tol = 5 + + step = np.median(np.diff(xvalues)) + tol = step / 2 + + xvalues_grid = [xvalues[0]] + i = 1 + while i < len(xvalues): + previous = xvalues_grid[-1] + new = xvalues[i] + + # detect a large jump + if abs(new - previous) > 2 * step + tol: + xvalues_grid.append(new) + i += 1 + # detect a small jump + elif abs(new - previous) > step + tol: + #print("adding intermediate between", previous, new,"because",new-previous,">", step+tol) + xvalues_grid.append(previous + step) + else: + xvalues_grid.append(new) + i += 1 + xvalues_grid = np.array(xvalues_grid) - xvalues_grid = get_uniform_grid(xvalues) + #xvalues_grid = get_uniform_grid(xvalues) step = np.median(np.diff(xvalues_grid)) # valid points are no more than 2*step from actual data. - valid = np.any(np.abs(xvalues_grid[:, None] - xvalues[None, :]) <= 2 * step, axis=1) + valid = np.any(np.abs(xvalues_grid[:, None] - xvalues[None, :]) < 2 * step, axis=1) # if np.sum(~valid): # print("Warning: removing some values before interpolation!") @@ -77,11 +102,6 @@ def interpolate_parts(xvalues, values, step=None, verbose=False): print(f"interpolating at {np.sum(valid)} points") xvalues_grid = xvalues_grid[valid] - assert np.abs(np.median(np.diff(xvalues_grid)) - step) < 1e-10, ( - np.median(np.diff(xvalues_grid)), - step, - ) - interpolator = scipy.interpolate.interp1d( xvalues, values, kind="linear", fill_value="extrapolate" ) @@ -118,7 +138,9 @@ def add_data(self, slices, values, stds=None, distances=None, mics=range(4)): :param stds: standard deviations (n_mics, ) """ assert slices.shape[1] == len(values), slices.shape - self.slices = slices + self.slices = {} + for mic_idx, slice_ in zip(mics, slices): + self.slices[mic_idx] = slice_ self.values = values self.stds = stds self.distances_cm = distances @@ -145,8 +167,9 @@ def calibrate(self): self.valid_idx &= valid_idx f_calib = self.calibration_function(self.values[self.valid_idx]) - f_calib_mics = f_calib[self.mics, :] - self.slices[:, self.valid_idx] /= f_calib_mics + #f_calib_mics = f_calib[self.mics, :] + for k in self.slices.keys(): + self.slices[k][self.valid_idx] /= f_calib[k] self.is_calibrated = True def filter_out_freqs(self, freq_ranges=BAD_FREQ_RANGES, verbose=False): @@ -162,26 +185,34 @@ def filter_out_freqs(self, freq_ranges=BAD_FREQ_RANGES, verbose=False): if verbose: print("number of frequencies after:", np.sum(self.valid_idx)) - def do_inference(self, algorithm, mic_idx, calibrate=True, normalize=True, ax=None): + def do_inference(self, algorithm, mic_idx, calibrate=True, normalize=True, ax=None, + interpolate=INTERPOLATE): """ Perform distance inference on current data. """ + from copy import deepcopy if calibrate and not self.is_calibrated: self.calibrate() - valid = self.valid_idx & np.all(~np.isnan(self.slices), axis=0) + valid = deepcopy(self.valid_idx) + for mic, slice_ in self.slices.items(): + valid = valid & (~np.isnan(slice_)) + #valid = self.valid_idx & np.all(~np.isnan(self.slices), axis=0) if algorithm == "bayes": sigma = self.stds[mic_idx] if self.stds is not None else None dists_cm, proba, diffs_cm = get_probability_bayes( - self.slices[mic_idx, valid] ** 2, + self.slices[mic_idx][valid] ** 2, self.values[valid], mic_idx=mic_idx, distance_range=self.distance_range_cm, sigma=sigma, azimuth_deg=self.azimuth_deg, + interpolate=interpolate ) elif algorithm == "cost": + if interpolate: + print("Warning: interpolating for cost algorithm doesn't make sense.") distances_cm = ( self.distances_cm[valid] if self.distances_cm is not None else None ) @@ -192,7 +223,7 @@ def do_inference(self, algorithm, mic_idx, calibrate=True, normalize=True, ax=No diffs_m, __ = get_deltas_from_global(self.azimuth_deg, dists_cm, mic_idx) diffs_cm = diffs_m * 1e2 proba = get_probability_cost( - self.slices[mic_idx, valid] ** 2, + self.slices[mic_idx][valid] ** 2, self.values[valid], dists_cm, mic_idx=mic_idx, @@ -213,7 +244,7 @@ def do_inference(self, algorithm, mic_idx, calibrate=True, normalize=True, ax=No diffs_cm = diffs_m * 1e2 azimuth_degs = np.arange(360, step=10) proba_2d = get_probability_cost_2d( - self.slices[mic_idx, valid], + self.slices[mic_idx][valid], self.values[valid], dists_cm, mic_idx=mic_idx, @@ -241,12 +272,15 @@ def do_inference(self, algorithm, mic_idx, calibrate=True, normalize=True, ax=No def plot(self, i_mic, ax, label=None, standardize=False, **kwargs): from copy import deepcopy - slice_mic = deepcopy(self.slices[i_mic, self.valid_idx]) + slice_mic = deepcopy(self.slices[i_mic][self.valid_idx]) if standardize: slice_mic = standardize_vec(slice_mic) ax.plot( - self.values[self.valid_idx], slice_mic, label=label, **kwargs, + self.values[self.valid_idx], + slice_mic, + label=label, + **kwargs, ) @@ -274,12 +308,12 @@ def convert_differences_to_distances(differences_cm, mic_idx, azimuth_deg): def get_posterior(abs_fft, sigma=None, data=None): N = len(abs_fft) - periodogram = 1 / N * abs_fft ** 2 + periodogram = 1 / N * abs_fft**2 # print('periodogram:', np.min(periodogram), np.max(periodogram)) if sigma is not None: if np.any(sigma > 0): - periodogram /= sigma ** 2 + periodogram /= sigma**2 # TODO(FD) we do below for numerical reasons. its effect # is undone by later exponentiation anyways. Make sure # this really as no effect on the result. @@ -322,12 +356,16 @@ def get_probability_bayes( if interpolate: frequencies_grid, f_slice_grid = interpolate_parts( - frequencies, f_slice, # step=20 + frequencies, + f_slice, # step=20 ) + #print("freqs = ", repr(frequencies)) + #print("freqs_interp = ", repr(frequencies_grid)) abs_fft = get_abs_fft(f_slice_grid, n_max=n_max) differences = get_differences(frequencies_grid, n_max=n_max) posterior = get_posterior(abs_fft, sigma, data=f_slice_grid) else: + print("Warning: not interpolating") abs_fft = get_abs_fft(f_slice, n_max=n_max) # get path interference differences corresponding to used frequencies @@ -422,7 +460,12 @@ def get_probability_cost( def get_periods_fft( - d_slice, frequency, relative_distances_cm, n_max=N_MAX, bayes=False, sigma=None, + d_slice, + frequency, + relative_distances_cm, + n_max=N_MAX, + bayes=False, + sigma=None, ): # the distribution over measured period. d_m = np.mean(np.diff(relative_distances_cm)) * 1e-2 @@ -456,7 +499,7 @@ def get_approach_angle_fft( interpolate=INTERPOLATE, factor=2, ): - """ + """ Get probabilities over approach angles. :param d_slice: amplitude measurements along distance @@ -546,3 +589,37 @@ def get_approach_angle_cost( probs_angle = np.nanmax(probs, axis=0) # take maximum across distances probs_angle /= np.nansum(probs_angle) return probs_angle + + +def get_1d_spectrum(spec): + """Get one-dimensional angle distribution from freq-angle spectrum. + + :param spec: spectrum of shape n_freqs x n_angles + :returns: a distribution of length n_angles + + """ + tol = max(np.min(spec[spec > 0]), 1e-2) + spec[spec < tol] = tol + vals = np.sum(np.log10(spec), axis=0) + if ((np.nanmax(vals) - np.nanmin(vals)) > 0): + vals = (vals - np.nanmin(vals)) / (np.nanmax(vals) - np.nanmin(vals)) + return vals + + +def get_angle_distribution(signals_f, frequencies, mics, cancel_centre=True): + from audio_stack.beam_former import BeamFormer + + assert ( + mics.shape[1] == signals_f.shape[1] + ), "mics should be dxN, signals_df should be FxN" + assert ( + len(frequencies) == signals_f.shape[0] + ), "signals_df should be FxN, frequencies of length F" + + beamformer = BeamFormer(mic_positions=mics.T) + R = beamformer.get_correlation(signals_f) + spectrum = beamformer.get_lcmv_spectrum( + R, frequencies_hz=frequencies, extra_constraints=[], cancel_centre=cancel_centre + ) + probs = get_1d_spectrum(spectrum) + return beamformer.theta_scan_deg, probs diff --git a/python/utils/moving_estimators.py b/python/utils/moving_estimators.py index 694db058..50190f89 100644 --- a/python/utils/moving_estimators.py +++ b/python/utils/moving_estimators.py @@ -1,17 +1,20 @@ #! /usr/bin/env python3 # -*- coding: utf-8 -*- - """ -moving_estimators.py: Integrate moving into the distance- and angle estimation. +moving_estimators.py: Provides moving window filter (MovingEstimator) class. """ import warnings import numpy as np -import scipy.interpolate import scipy.signal -from .geometry import Context - +from .base_estimator import ( + BaseEstimator, + get_normal_vector, + get_estimate, + get_estimates, + from_0_to_360, +) # RELATIVE_MOVEMENT_STD implicityly defines the forgetting factor when # combining multiple measurements. The relative weights of the @@ -24,214 +27,11 @@ # [1, 0.99, 0.98, 0.97, 0.96] # for 0: # [1, 1, 1, 1, 1] -RELATIVE_MOVEMENT_STD = 0.3 # - - -def get_std_sample(values, probs, means, unbiased=True): - if np.isclose(np.nansum(probs), 1) or ( - np.nansum(probs) < 1 - ): # for distributions (might be unnormalized) - norm = 1.0 - else: # for histograms - norm = ( - (np.nansum(probs) - 1) if unbiased else np.nansum(probs) - ) # for histograms - if not norm: - return None if np.ndim(means) == 0 else np.full(len(means), None) - - if np.ndim(means) > 0: - coeffs = np.multiply(probs[:, None], (values[:, None] - means[None, :]) ** 2) - var = np.nansum(coeffs, axis=0) / norm - else: - var = np.nansum(np.multiply(probs, (values - means) ** 2), axis=0) / norm - return np.sqrt(var) - - -def get_std_of_peaks(values, probs, peaks): - widths, *__ = scipy.signal.peak_widths(probs, peaks) - fwhm = widths * (values[1] - values[0]) # assumes uniform values - return fwhm / 2 / np.sqrt(2 * np.log(2)) - - -def get_estimate(values, probs, method, unbiased=True): - # distirbution with only one value, return that value. - if len(values) == 1: - return values[0], 0.0 - - if np.nansum(probs) == 0: - return None, None - - if method == "mean": - mean = np.nansum(np.multiply(probs, values)) / np.nansum(probs) - std = get_std_sample(values, probs, means=mean, unbiased=unbiased) - return mean, std - elif method == "max": - max_prob = np.nanmax(probs) - estimates = values[np.where(probs == max_prob)[0]] - stds = get_std_sample(values, probs, means=estimates, unbiased=unbiased) - elif method == "peak": - indices, __ = scipy.signal.find_peaks(probs) - if not len(indices): - return None, None - prob_estimates = probs[indices] - max_prob = np.max(prob_estimates) - indices = np.where(probs == max_prob)[0] - estimates = values[indices] - stds = get_std_of_peaks(values, probs, indices) - else: - raise ValueError(method) - - if len(estimates) == len(probs): - # print(f"Warning: uniform distribution?") - return None, None - elif len(estimates) > 1: - pass - # print(f"Warning: ambiguous distribution, {len(estimates)} maxima") - elif not len(estimates): - # print(f"Warning: no valid estimates detected: {values}, {probs}") - return None, None - return estimates[0], stds[0] - - -def get_estimates(values, probs, method, sort=True, n_estimates=None): - """ - :param n_estimates: number of estimates (i.e. peaks) to return. None: return all. - :param sort: sort estimates by probability (most likely first). - """ - if n_estimates == 1: - mean, std = get_estimate(values, probs, method=method) - return [mean], [std] - if n_estimates is None or n_estimates > 1: - if method != "peak": - warnings.warn( - f"Using method peak instead of {method} for n_estimates={n_estimates}" - ) - - indices, properties = scipy.signal.find_peaks(probs, width=True) - if n_estimates and len(indices) < n_estimates: - warnings.warn("Found less peaks than requested") - return None, None - stds = get_std_of_peaks(indices) - - estimates = values[indices] - prob_estimates = probs[indices] - if sort: - sort_indices = prob_estimates.argsort()[::-1] - estimates = estimates[sort_indices] - prob_estimates = prob_estimates[sort_indices] - stds = stds[sort_indices] - - if n_estimates is None: - return estimates, stds - else: - return estimates[:n_estimates], stds[:n_estimates] - - -def from_0_to_360(angle): - angle %= 360 - angle = angle + 360 if angle < 0 else angle - return angle +RELATIVE_MOVEMENT_STD = 0.0 # def get_normal(angle_deg): - return -np.r_[ - np.cos(angle_deg / 180 * np.pi), np.sin(angle_deg / 180 * np.pi), - ] - - -class BaseEstimator(object): - def __init__( - self, n_window=2, platform="crazyflie", - ): - self.difference_p = {n: {} for n in range(n_window)} - self.positions = np.full((n_window, 2), None) - self.rotations = np.full(n_window, None) - self.times = np.full(n_window, None) - - self.dim = 2 - self.reference = None - - self.counter = 0 # total counter - self.index = -1 # rolling index - self.n_window = n_window - self.filled = False - - self.context = Context.get_platform_setup(platform) - - def add_distributions( - self, diff_dictionary, position_cm=[0, 0], rot_deg=0, - ): - """ - diff_dictionary: - { - mic_idx1: (diff1_cm, prob1), - mic_idx2: (diff2_cm, prob2), - ... - } - """ - if len(position_cm) > 2: - position_cm = position_cm[:2] - - self.index += 1 - if self.index == self.n_window - 1: - self.filled = True - elif self.index == self.n_window: # rolling window - self.index = 0 - - self.positions[self.index, :] = position_cm - self.rotations[self.index] = rot_deg - self.times[self.index] = self.counter - self.counter += 1 - - for mic_idx, (diff, prob) in diff_dictionary.items(): - if not any(diff > 1): - print("Warning: need to pass diff in centimeters") - self.difference_p[self.index][mic_idx] = scipy.interpolate.interp1d( - diff.astype(float), - prob.astype(float), - fill_value=0, - bounds_error=False, - kind="linear", - ) - - def enough_measurements(self): - return self.filled - - def get_ordered_index(self): - """ Get the list of indices that the current elements in the buffer correspond to. - For instance, if self.positions contains: [p7 p1 p2 p3 p4 p5 p6] - it returns ordered_index=[6, 0, 1, 2, 3, 4, 5], such that positions[ordered_index] - corresponds to the positions ordered chronologically: [p1, p2, p3, p4, p5, p6, p7], - with the oldest position first. - """ - if self.filled: - n_elements = self.n_window - else: - n_elements = self.index + 1 - ordered_index = np.roll(range(n_elements), -self.index - 1) - return ordered_index - - def get_forward_angle(self): - if self.positions[self.index][1] < 0: - return 270 - else: - return 90 - - def get_local_forward_angle(self): - current = self.index - if not self.filled and (self.index < 2): - # print("Warning: cannot simplify angles yet because buffer not filled.") - return 0 - # if self.positions is [p5, p6, p2, p3, p4] with current=2, return [p2, p3, p4, p5, p6] - ordered_positions = self.positions[self.get_ordered_index()] - - forward_dir_global = np.mean(np.diff(ordered_positions, axis=0), axis=0) - angle_global_deg = ( - 180 / np.pi * np.arctan2(forward_dir_global[1], forward_dir_global[0]) - ) - angle_local_deg = angle_global_deg - self.rotations[current] - # print(f"local angle: {angle_local_deg:.1f} deg") - return angle_local_deg + return -get_normal_vector(angle_deg) class MovingEstimator(BaseEstimator): @@ -243,7 +43,6 @@ class MovingEstimator(BaseEstimator): # ESTIMATION_METHOD = "peak" # ESTIMATION_METHOD = "mean" - ESTIMATION_METHOD = "max" def __init__( @@ -255,7 +54,8 @@ def __init__( relative_movement_std=RELATIVE_MOVEMENT_STD, ): super().__init__( - n_window=n_window, platform=platform, + n_window=n_window, + platform=platform, ) self.relative_movement_std = relative_movement_std @@ -266,7 +66,7 @@ def get_distributions(self, verbose=False, simplify_angles=True, plot_angles=[]) def clean_distribution(dist): if not np.any(~np.isnan(dist)): dist = np.ones_like(dist) - sum_ = np.sum(dist) + # sum_ = np.sum(dist) # if sum_: # dist /= sum_ return dist @@ -276,12 +76,11 @@ def clean_distribution(dist): verbose=verbose, simplify_angles=simplify_angles, plot_angles=plot_angles ) - # softmax # the more 'peaky', the better the distance measurements match for, therefore the more likely # this angle is the correct one. # probs_angles = np.exp(np.max(distributions, axis=1)) # softmax - probs_angles = np.max(distributions, axis=1) # probs_angles = np.sum(distributions, axis=1) + probs_angles = np.max(distributions, axis=1) probs_angles = clean_distribution(probs_angles) if verbose: @@ -345,10 +144,10 @@ def get_joint_distribution( else: # if we are at 2, use [0, 1, 3, 4] (n_window=5) others = list(range(self.n_window)) - others.remove(current) + others.remove(self.index) # search only around the forward direction instead of all 360 degrees. This makes sense - # because the only "dangerous" angles are in front of us. + # because the only "dangerous" walls are in front of us. if simplify_angles: # angle_local_deg = self.get_local_forward_angle() # angles_deg = np.arange( @@ -426,10 +225,13 @@ def get_joint_distribution( current_time = self.times[self.index] lambdas = [] for i_time, probs_mics in distance_p.items(): # all measurement times + # account for movement uncertainty. this gives more weight to + # most recent measurements. The higher the movement std the + # higher the dropoff. if len(probs_mics): time_lag = current_time - self.times[i_time] lambdas.append( - 1 / (1 + self.relative_movement_std ** 2) ** time_lag + 1 / (1 + self.relative_movement_std**2) ** time_lag ) for mic, probs in probs_mics.items(): # all mics if verbose and angle_local in plot_angles: diff --git a/python/utils/pandas_utils.py b/python/utils/pandas_utils.py index 5448fae8..3dbc6b33 100644 --- a/python/utils/pandas_utils.py +++ b/python/utils/pandas_utils.py @@ -8,6 +8,14 @@ import numpy as np import pandas as pd +from .plotting_tools import make_dirs + +def save_pickle(results_df, fname): + results_df = results_df.apply(pd.to_numeric, axis=0, errors="ignore",downcast='integer') + make_dirs(fname) + pd.to_pickle(results_df, fname) + print("saved as", fname) + def filter_by_dict(df, dict_): """ Return the rows of df for which dict_ applies. """ diff --git a/python/utils/particle_estimators.py b/python/utils/particle_estimators.py index 2944ff98..20f4b28f 100644 --- a/python/utils/particle_estimators.py +++ b/python/utils/particle_estimators.py @@ -2,20 +2,15 @@ # -*- coding: utf-8 -*- """ -particle_estimators.py: +particle_estimators.py: Provides particle filter implementation """ from enum import Enum import numpy as np from scipy.stats import norm -from utils.moving_estimators import BaseEstimator - - -def get_normal_vector(angle_deg): - return np.r_[ - np.cos(angle_deg / 180 * np.pi), np.sin(angle_deg / 180 * np.pi), - ] +from utils.base_estimator import BaseEstimator, get_normal_matrix, from_0_to_360 +from utils.constants import PLATFORM # simplest, but expensive and might miss important particles: @@ -25,30 +20,36 @@ def get_normal_vector(angle_deg): # very uniform, doesn't miss important samples: from filterpy.monte_carlo import stratified_resample as resample -# corresponds to discretization "fine": -DISTANCES_CM = np.arange(7, 80, step=2.0) -ANGLES_DEG = np.arange(360, step=10) - +DISTANCES_CM = np.arange(7, 80, step=1.0) +ANGLES_DEG = np.arange(360, step=10.0) STD_DISTANCE_CM = 5.0 -STD_ANGLE_DEG = 5.0 +STD_ANGLE_DEG = 20.0 -# ignore pose estimates and predict particles by adding +# if True, ignore pose estimates and predict particles by adding # uniform disturbance. -PREDICT_UNIFORM = True +PREDICT_UNIFORM = False + +INIT_UNIFORM = True +# always create this percentage of random particles to make sure we don't get +# get stuck in local minima. We replace the particles of lowest weight. +RANDOM_PARTICLE_RATIO = 0.1 class State(Enum): NEED_UPDATE = 0 NEED_PREDICT = 1 NEED_RESAMPLE = 2 READY = 3 + NEED_INIT = 4 def get_bins(centers): # centers: 1, 2, 3, 4 # bin_widths: 0.5, 0.5, 0.5 # bins = 0.5, 1.5, 2.5, + if len(centers) == 1: + return centers bin_widths = np.diff(centers) bins = np.r_[ centers[:-1] - bin_widths / 2, @@ -59,9 +60,8 @@ def get_bins(centers): return bins -def resample_from_index(particles, weights, indices): - particles[:] = particles[indices, :] - weights.resize(particles.shape[0]) +def resample_from_index(states, weights, indices): + states[:] = states[indices] weights.fill(1.0 / len(weights)) @@ -69,38 +69,23 @@ class ParticleEstimator(BaseEstimator): def __init__( self, n_particles, - platform="crazyflie", - global_=True, + platform=PLATFORM, distances_cm=DISTANCES_CM, angles_deg=ANGLES_DEG, predict_uniform=PREDICT_UNIFORM, ): super().__init__( - n_window=2, platform=platform, - ) - - self.n_particles = n_particles - self.particles = np.empty((n_particles, 2)) - self.particles[:, 0] = np.linspace( - distances_cm[0], distances_cm[-1], n_particles + n_window=2, + platform=platform, ) # yields an initial distribution that is not perfectly distributed # (more localized in a sphere round center) but it is good enough. - dd, aa = np.meshgrid(distances_cm, angles_deg) - self.particles[:, 0] = np.random.choice( - dd.flatten(), size=n_particles, replace=True - ) - self.particles[:, 1] = np.random.choice( - aa.flatten(), size=n_particles, replace=True - ) - + self.n_particles = n_particles + self.states = np.empty((n_particles, 2)) + self.weights = np.ones(n_particles) / n_particles - # express particles in global reference frame. - # if false, will express particles in local reference frame. - self.global_ = global_ - self.distances_cm = distances_cm self.angles_deg = angles_deg @@ -108,23 +93,56 @@ def __init__( # state machine to make sure predict, update and resample are done # in the correct order and not more than necessary. - self.state = State.READY + if INIT_UNIFORM: + dd, aa = np.meshgrid(distances_cm, angles_deg) + self.states[:, 0] = np.random.choice( + aa.flatten(), size=n_particles, replace=True + ) + self.states[:, 1] = np.random.choice( + dd.flatten(), size=n_particles, replace=True + ) + self.state = State.READY + else: + self.state = State.NEED_INIT def effective_n(self): - return 1.0 / np.sum(np.square(self.weights)) + return 1.0 / np.sum(self.weights ** 2) def add_distributions(self, *args, **kwargs): super().add_distributions(*args, **kwargs) - self.state = State.NEED_PREDICT + if (self.state != State.NEED_INIT): + self.state = State.NEED_PREDICT def get_distributions(self, simplify_angles=False, method="histogram"): + if simplify_angles: # sample angles from current direction. angle_deg = self.get_forward_angle() - self.particles[:, 1] = np.random.normal( + self.states[:, 0] = np.random.normal( loc=angle_deg, scale=STD_ANGLE_DEG, size=self.n_particles ) + # sample from the first measurements for initialization. + if (self.state == State.NEED_INIT): + # use only first mic and the approximation that delta = 2*d + print("initializing from measurements") + mics = list(self.difference_p[self.index].keys()) + difference_p = self.difference_p[self.index][mics[0]] + difference_hist = difference_p(self.distances_cm) + difference_hist /= np.sum(difference_hist) + self.states[:, 1] = np.random.choice(self.distances_cm, self.n_particles, p=difference_hist) + + # use angles from beamforming if possible, otherwise uniform. + if self.angle_probs[self.index] is not None: + print("using angles from beamforming") + angle_p = self.angle_probs[self.index] + angle_hist = angle_p(self.angles_deg) + angle_hist /= np.sum(angle_hist) + self.states[:, 0] = np.random.choice(self.angles_deg, self.n_particles, p=angle_hist) + else: + self.states[:, 0] = np.random.choice(self.angles_deg, self.n_particles) + self.state = State.NEED_PREDICT + if not self.state == State.READY: self.predict() self.update(simplify_angles=simplify_angles) @@ -132,14 +150,20 @@ def get_distributions(self, simplify_angles=False, method="histogram"): if method == "histogram": bins = get_bins(self.distances_cm) - probs_dist, __ = np.histogram( - self.particles[:, 0], bins=bins, weights=self.weights - ) + if len(bins) == 1: + probs_dist = [1.0] + else: + probs_dist, __ = np.histogram( + self.states[:, 1], bins=bins, weights=self.weights + ) bins = get_bins(self.angles_deg) - probs_angles, __ = np.histogram( - self.particles[:, 1], bins=bins, weights=self.weights - ) + if len(bins) == 1: + probs_angles = [1.0] + else: + probs_angles, __ = np.histogram( + self.states[:, 0], bins=bins, weights=self.weights + ) elif method == "gaussian": (mean_dist, mean_angle), (var_dist, var_angle) = self.estimate() norm_dist = norm(loc=mean_dist, scale=np.sqrt(var_dist)) @@ -155,109 +179,97 @@ def get_distributions(self, simplify_angles=False, method="histogram"): def update(self, simplify_angles=False): """ - Update particle weights based on path difference measurements. - - If particles are in global reference frame, use the equation: - a_local = a_global - rotation - d_local = d_global - n(a_global).dot(position) + Update particle weights based on path difference measurements. """ if not self.state == State.NEED_UPDATE: return - for i, (d_particle, a_particle) in enumerate(self.particles): - if self.global_: - normal_global = get_normal_vector(a_particle) - a_local = a_particle - self.rotations[self.index] - d_local = d_particle - normal_global.dot(self.positions[self.index]) - else: - a_local = a_particle - d_local = d_particle + for k, (a_local, d_local) in enumerate(self.states): # print(f"for global {d_particle:.1f}, local distance and angle: {d_local:.1f}, {a_local:.0f}") - weight = 1.0 + prob = 1.0 for mic, diff_dist in self.difference_p[self.index].items(): - # if mic == 0: - # print(f"maximum of interpolator: {diff_dist.x[np.argmax(diff_dist.y)]}") - delta = self.context.get_delta(a_local, d_local, mic) - weight *= diff_dist(delta) - # if mic == 0: - # print(f" m0: delta for angle {a_local:.0f} d {d_local:.0f}", diff_dist(delta)) - self.weights[i] *= weight + delta_local_cm = self.context.get_delta(a_local, d_local, mic_idx=mic) + prob *= diff_dist(delta_local_cm) # interpolate at delta + + if self.angle_probs[self.index] is not None: + prob *= self.angle_probs[self.index](a_local) # interpolate at angle + + self.weights[k] *= prob self.weights /= np.sum(self.weights) - self.state = State.NEED_RESAMPLE + # take the 30% of lowest weight particles and distribute them uniformly. + if RANDOM_PARTICLE_RATIO > 0: + n_uniform = int(round(self.n_particles * RANDOM_PARTICLE_RATIO)) + indices = np.argsort(self.weights) + self.states[indices[:n_uniform], 0] = np.random.choice(self.angles_deg, n_uniform) + self.states[indices[:n_uniform], 0] = np.random.choice(self.distances_cm, n_uniform) + + # more robust if we always resample. + if True: #self.effective_n() < self.states.shape[0] / 2: + self.state = State.NEED_RESAMPLE + else: + self.state = State.READY def predict(self): - """ Predict new particles based on movement estimates. - """ + """Predict new particles based on movement estimates.""" if not self.state == State.NEED_PREDICT: return - if (not self.predict_uniform) and self.filled and (not self.global_): + if (not self.predict_uniform) and self.filled: # rolling window with two elements: previous = 1 if self.index == 0 else 0 pos_delta = self.positions[self.index] - self.positions[previous] rot_delta = self.rotations[self.index] - self.rotations[previous] - for i in range(self.particles.shape[0]): - a_local = self.particles[i, 1] - d_corr = get_normal_vector(self.rotations[previous] + a_local).dot( - pos_delta - ) - self.particles[i, 0] -= d_corr - self.particles[i, 1] -= rot_delta - self.particles[:, 0] += np.random.normal( - scale=STD_DISTANCE_CM, size=self.n_particles - ) - self.particles[:, 1] += np.random.normal( + a_global = self.rotations[previous] + self.states[:, 0] + self.states[:, 0] = from_0_to_360(self.states[:, 0] - rot_delta) + self.states[:, 1] = self.states[:, 1] - get_normal_matrix(a_global) @ pos_delta + + self.states[:, 0] += np.random.normal( scale=STD_ANGLE_DEG, size=self.n_particles ) + self.states[:, 1] += np.random.normal( + scale=STD_DISTANCE_CM, size=self.n_particles + ) - self.particles[:, 0] = np.clip( - self.particles[:, 0], + self.states[:, 1] = np.clip( + self.states[:, 1], a_min=self.distances_cm[0], a_max=self.distances_cm[-1], ) - self.particles[:, 1] %= 360 self.state = State.NEED_UPDATE def estimate(self): - """ Returns mean and variance of the weighted particles. """ + """Returns mean and variance of the weighted particles.""" import scipy.stats if not self.state == State.READY: print("Warning: not ready for estimation yet.") return None - mean_distance = np.average(self.particles[:, 0], weights=self.weights) + mean_distance = np.average(self.states[:, 1], weights=self.weights) var_distance = np.average( - (self.particles[:, 0] - mean_distance) ** 2, weights=self.weights + (self.states[:, 1] - mean_distance) ** 2, weights=self.weights ) sin_ = np.sum( - np.multiply(np.sin(self.particles[:, 1] / 180 * np.pi), self.weights) + np.multiply(np.sin(self.states[:, 0] / 180 * np.pi), self.weights) ) cos_ = np.sum( - np.multiply(np.cos(self.particles[:, 1] / 180 * np.pi), self.weights) + np.multiply(np.cos(self.states[:, 0] / 180 * np.pi), self.weights) ) mean_angle = 180 / np.pi * np.arctan2(sin_, cos_) mean_angle %= 360 - var_angle = 180 / np.pi * np.sqrt(sin_ ** 2 + cos_ ** 2) + var_angle = 180 / np.pi * np.sqrt(sin_**2 + cos_**2) return (mean_distance, mean_angle), (var_distance, var_angle) def resample(self): if not self.state == State.NEED_RESAMPLE: return - - if True: # self.effective_n() < self.particles.shape[0] / 2: - try: - indices = resample(self.weights) - except: - print(self.weights) - raise - resample_from_index(self.particles, self.weights, indices) - else: - print("Warning: not resampling") - pass - + try: + indices = resample(self.weights) + except: + raise + resample_from_index(self.states, self.weights, indices) self.state = State.READY diff --git a/python/utils/simulation.py b/python/utils/simulation.py index 5643efc4..df1830e5 100644 --- a/python/utils/simulation.py +++ b/python/utils/simulation.py @@ -1,7 +1,6 @@ import sys import numpy as np -import pyroomacoustics as pra from audio_stack.beam_former import rotate_mics from audio_simulation.geometry import ROOM_DIM @@ -11,9 +10,11 @@ if PLATFORM == "epuck": from epuck_description_py.parameters import N_BUFFER, FS from epuck_description_py.experiments import WALL_ANGLE_DEG_STEPPER + WIDEBAND_FILE = "results/wideband_epuck.npy" else: from crazyflie_description_py.parameters import N_BUFFER, FS from crazyflie_description_py.experiments import WALL_ANGLE_DEG_STEPPER + WIDEBAND_FILE = "results/wideband_crazyflie.npy" from .frequency_analysis import get_bin from .geometry import * from .signals import generate_signal @@ -21,7 +22,6 @@ # default wall absorption (percentage of amplitude that is lost in reflection): WALL_ABSORPTION = 0.2 GAIN = 1.0 # default amplitude for input signals -WIDEBAND_FILE = "results/wideband.npy" N_TIMES = 10 # number of buffers to use for average (pyroomacoutics) @@ -54,42 +54,51 @@ def simulate_distance_estimator( return distance_estimator -def create_wideband_signal(frequencies, duration_sec=1.0): +def create_wideband_signal(frequencies, duration_sec=2.0): phase = np.random.uniform(0, 2 * np.pi) - kwargs = dict(signal_type="mono", duration_sec=duration_sec, Fs=FS,) - signal = generate_signal(frequency_hz=frequencies[1], phase_offset=phase, **kwargs) - for f in frequencies[2:]: + kwargs = dict( + signal_type="mono", + duration_sec=duration_sec, + Fs=FS, + ) + signal = generate_signal(frequency_hz=frequencies[0], phase_offset=phase, **kwargs) + for f in frequencies[1:]: phase = np.random.uniform(0, 2 * np.pi) signal += generate_signal(frequency_hz=f, phase_offset=phase, **kwargs) return signal def generate_room( - distance_cm=0, azimuth_deg=WALL_ANGLE_DEG_STEPPER, ax=None, fs_here=FS + distance_cm=0, azimuth_deg=WALL_ANGLE_DEG_STEPPER, ax=None, fs_here=FS, source=True, chosen_mics=None ): + import pyroomacoustics as pra + """ Generate two-dimensional setup using pyroomacoustics. """ - source, mic_positions = get_setup(distance_cm, azimuth_deg, ax) + source_position, mic_positions = get_setup(distance_cm, azimuth_deg, ax) + if chosen_mics is not None: + mic_positions = mic_positions[chosen_mics, :] m = pra.Material(energy_absorption="glass_3mm") room = pra.ShoeBox(fs=fs_here, p=ROOM_DIM[:2], max_order=1, materials=m) beam_former = pra.Beamformer(mic_positions.T, room.fs) room.add_microphone_array(beam_former) - room.add_source(source) + if source: + room.add_source(source_position) return room def get_setup(distance_cm=0, azimuth_deg=WALL_ANGLE_DEG_STEPPER, ax=None, zoom=True): - """ Create a setup for pyroomacoustics that corresponds to distance_cm and azimuth_deg""" + """Create a setup for pyroomacoustics that corresponds to distance_cm and azimuth_deg""" context = Context.get_platform_setup() d_wall_m = distance_cm * 1e-2 # distance of wall - offset = [ROOM_DIM[0] - d_wall_m, ROOM_DIM[1] / 2] # location of drone + offset = [ROOM_DIM[0] - d_wall_m, ROOM_DIM[1] / 2] # location of robot mic_positions = context.mics source = context.source + offset - # note that we need to take the negative azimuth, because the drone has to + # note that we need to take the negative azimuth, because the robot has to # be moved in the opposite direction. mic_positions = offset + rotate_mics(mic_positions, -azimuth_deg) @@ -150,32 +159,33 @@ def get_df_theory_simple( frequencies_hz = np.array(frequencies_hz).reshape((1, -1)) mag_squared = ( - alpha0 ** 2 - + alpha1 ** 2 + alpha0**2 + + alpha1**2 + 2 * alpha0 * alpha1 * np.cos(2 * np.pi * frequencies_hz * deltas_m / c) ) # n_deltas x n_freqs or n_freqs return mag_squared * gain -def get_average_magnitude(room, signal, n_buffer=N_BUFFER, n_times=N_TIMES): - """ +def get_average_signals(room, signal, n_buffer=N_BUFFER, n_times=N_TIMES): + """ :param signal: signal to use for simulation, array :param n_buffer: buffer size for "STFT" :param n_times: number of buffers to average. Will start fromsecond buffer. """ buffers_f = get_buffers(room, signal, n_buffer, n_times) - return np.mean(np.abs(buffers_f), axis=0) + return np.mean(buffers_f, axis=0) def get_buffers(room, signal, n_buffer=N_BUFFER, n_times=N_TIMES): - """ + """ :param signal: signal to use for simulation, array :param n_buffer: buffer size for "STFT" :param n_times: number of buffers to average. Will start fromsecond buffer. """ - assert len(room.sources) == 1 - room.sources[0].add_signal(signal) + for i in range(len(room.sources)): + if room.sources[i].signal is None: + room.sources[i].add_signal(signal) room.simulate() assert (n_times * n_buffer) < room.mic_array.signals.shape[ @@ -214,11 +224,12 @@ def get_freq_slice_pyroom( signal, azimuth_deg=WALL_ANGLE_DEG_STEPPER, chosen_mics=None, + return_complex=False ): - room = generate_room(distance_cm=distance_cm, azimuth_deg=azimuth_deg) + room = generate_room(distance_cm=distance_cm, azimuth_deg=azimuth_deg, chosen_mics=chosen_mics) n_times = len(signal) // N_BUFFER - mag = get_average_magnitude( + avg_signal = get_average_signals( room, signal, n_buffer=N_BUFFER, n_times=n_times ) # n_mics x n_frequencies freqs_all = np.fft.rfftfreq(N_BUFFER, 1 / FS) @@ -226,20 +237,24 @@ def get_freq_slice_pyroom( bins_ = [get_bin(freqs_all, f) for f in frequencies] else: bins_ = np.arange(len(frequencies)) - if chosen_mics is None: - return mag[:, bins_] ** 2 + + if return_complex: + return avg_signal[:, bins_] else: - return mag[chosen_mics][:, bins_] ** 2 + return np.abs(avg_signal)[:, bins_] ** 2 def get_dist_slice_pyroom( - frequency, distances_cm, azimuth_deg=WALL_ANGLE_DEG_STEPPER, n_times=100 + frequency, distances_cm, azimuth_deg=WALL_ANGLE_DEG_STEPPER, n_times=100, return_complex=False ): from .frequency_analysis import get_bin duration_sec = N_BUFFER * n_times / FS signal = generate_signal( - FS, duration_sec=duration_sec, signal_type="mono", frequency_hz=frequency, + FS, + duration_sec=duration_sec, + signal_type="mono", + frequency_hz=frequency, ) freqs_all = np.fft.rfftfreq(N_BUFFER, 1 / FS) bin_ = get_bin(freqs_all, frequency) @@ -247,17 +262,20 @@ def get_dist_slice_pyroom( Hs = [] for d in distances_cm: room = generate_room(distance_cm=d, azimuth_deg=azimuth_deg) - mag = get_average_magnitude(room, signal, n_buffer=N_BUFFER, n_times=n_times) - Hs.append(mag[:, bin_] ** 2) + avg_signal = get_average_signals(room, signal, n_buffer=N_BUFFER, n_times=n_times) + if return_complex: + Hs.append(avg_signal[:, bin_]) + else: + Hs.append(np.abs(avg_signal)[:, bin_] ** 2) return np.array(Hs) def get_freq_slice_theory( frequencies, distance_cm, azimuth_deg=WALL_ANGLE_DEG_STEPPER, chosen_mics=range(4) ): - """ + """ We can incorporate relative movement by providing - distance_cm and azimuth_deg of same length as frequencies. + distance_cm and azimuth_deg of same length as frequencies. """ Hs = np.zeros((len(frequencies), len(chosen_mics))) for i, mic in enumerate(chosen_mics): @@ -277,9 +295,9 @@ def get_dist_slice_theory( wall_absorption=WALL_ABSORPTION, gains=[GAIN] * 4, ): - """ + """ We can incorporate relative movement by providing - distance_cm and azimuth_deg of same length as frequencies. + distance_cm and azimuth_deg of same length as frequencies. """ if np.ndim(gains) == 0: gains = [gains] * len(chosen_mics) diff --git a/python/utils/split_particle_estimators.py b/python/utils/split_particle_estimators.py new file mode 100644 index 00000000..eb58119e --- /dev/null +++ b/python/utils/split_particle_estimators.py @@ -0,0 +1,252 @@ +#! /usr/bin/env python3 +# -*- coding: utf-8 -*- + +""" +particle_estimators.py: Provides particle filter implementation +""" +from enum import Enum + +import numpy as np +from scipy.stats import norm + +from utils.base_estimator import BaseEstimator, get_normal_matrix, from_0_to_360, get_estimates + +from filterpy.monte_carlo import stratified_resample as resample + +from utils.particle_estimators import resample_from_index, get_bins, State +from utils.particle_estimators import RANDOM_PARTICLE_RATIO, INIT_UNIFORM, PREDICT_UNIFORM, STD_DISTANCE_CM, STD_ANGLE_DEG +from utils.particle_estimators import DISTANCES_CM, ANGLES_DEG + +USE_ANGLE_PARTICLE = False + +def get_histogram(values, states, weights): + bins = get_bins(values) + if len(bins) == 1: + probs = [1.0] + else: + probs, __ = np.histogram(states, bins=bins, weights=weights) + return probs + +class SplitParticleEstimator(BaseEstimator): + def __init__( + self, + n_particles, + platform="crazyflie", + distances_cm=DISTANCES_CM, + angles_deg=ANGLES_DEG, + predict_uniform=PREDICT_UNIFORM, + ): + super().__init__( + n_window=2, + platform=platform, + ) + + # yields an initial distribution that is not perfectly distributed + # (more localized in a sphere round center) but it is good enough. + self.n_particles = n_particles + self.states = np.empty((n_particles, 2)) + self.weights_d = np.ones(n_particles) / n_particles + self.weights_a = np.ones(n_particles) / n_particles + + self.distances_cm = distances_cm + self.angles_deg = angles_deg + + self.predict_uniform = predict_uniform + + # state machine to make sure predict, update and resample are done + # in the correct order and not more than necessary. + if INIT_UNIFORM: + self.states[:, 0] = np.random.choice( + self.angles_deg, size=n_particles, replace=True + ) + + self.states[:, 1] = np.random.choice( + self.distances_cm, size=n_particles, replace=True + ) + self.state = State.READY + else: + self.state = State.NEED_INIT + + def effective_n(self, weights): + return 1.0 / np.sum(np.square(weights)) + + def add_distributions(self, *args, **kwargs): + super().add_distributions(*args, **kwargs) + if (self.state != State.NEED_INIT): + self.state = State.NEED_PREDICT + + def get_distributions(self, simplify_angles=False, method="histogram"): + + if simplify_angles: + # sample angles from current direction. + angle_deg = self.get_forward_angle() + self.states[:, 0] = np.random.normal( + loc=angle_deg, scale=STD_ANGLE_DEG, size=self.n_particles + ) + + # sample from the first measurements for initialization. + if (self.state == State.NEED_INIT): + # use only first mic and the approximation that delta = 2*d + print("initializing from measurements") + mics = list(self.difference_p[self.index].keys()) + difference_p = self.difference_p[self.index][mics[0]] + difference_hist = difference_p(self.distances_cm) + difference_hist /= np.sum(difference_hist) + self.states[:, 1] = np.random.choice(self.distances_cm, self.n_particles, p=difference_hist) + + # use angles from beamforming if possible, otherwise uniform. + if self.angle_probs[self.index] is not None: + print("using angles from beamforming") + angle_p = self.angle_probs[self.index] + angle_hist = angle_p(self.angles_deg) + angle_hist /= np.sum(angle_hist) + self.states[:, 0] = np.random.choice(self.angles_deg, self.n_particles, p=angle_hist) + else: + self.states[:, 0] = np.random.choice(self.angles_deg, self.n_particles) + self.state = State.NEED_PREDICT + + if not self.state == State.READY: + self.predict() + self.update(simplify_angles=simplify_angles) + self.resample() + + if method == "histogram": + probs_angles = get_histogram(self.angles_deg, self.states[:, 0], self.weights_a) + probs_dist = get_histogram(self.distances_cm, self.states[:, 1], self.weights_d) + elif method == "gaussian": + (mean_dist, mean_angle), (var_dist, var_angle) = self.estimate() + norm_dist = norm(loc=mean_dist, scale=np.sqrt(var_dist)) + probs_dist = norm_dist.pdf(self.distances_cm) + probs_dist /= np.sum(probs_dist) + + norm_angles = norm(loc=mean_angle, scale=np.sqrt(var_angle)) + probs_angles = norm_angles.pdf(self.angles_deg) + probs_angles /= np.sum(probs_angles) + else: + raise ValueError(method) + return self.distances_cm, probs_dist, self.angles_deg, probs_angles + + def update(self, simplify_angles=False): + """ + Update particle weights based on path difference measurements. + """ + if not self.state == State.NEED_UPDATE: + return + + #probs_angles = np.empty(self.n_particles) + for k, a_local in enumerate(self.states[:, 0]): + if self.angle_probs[self.index] is not None: + prob = self.angle_probs[self.index](a_local) # interpolate at angle + #probs_angles[k] = prob + + self.weights_a[k] *= prob + self.weights_a /= np.sum(self.weights_a) + + # get most likely angles + probs_angles = get_histogram(self.angles_deg, self.states[:, 0], self.weights_a) + candidate_angles, __ = get_estimates(self.angles_deg, probs_angles, method="peak", n_estimates=1) + + # get most likey angles based on beamforming only + #candidate_angles = [self.states[np.argmax(probs_angles), 0]] + + if not len(candidate_angles): + print("Warning: did not find any valid angles. Using uniform") + candidate_anlges = self.angles_deg + elif (len(candidate_angles) == 1) and (candidate_angles[0] is None): + print("Warning: no valid estimtae. Using uniform") + + candidate_anlges = self.angles_deg + for k, d_local in enumerate(self.states[:, 1]): + prob = 1.0 + for mic, diff_dist in self.difference_p[self.index].items(): + + ## expectation over all angles: + prob_angle = 0.0 + for a_local in candidate_angles: + if a_local is None: + print("Warning: angle is None") + continue + delta_local_cm = self.context.get_delta(a_local, d_local, mic_idx=mic) + prob_angle += diff_dist(delta_local_cm) # interpolate at delta + prob *= prob_angle / len(candidate_angles) + self.weights_d[k] *= prob + self.weights_d /= np.sum(self.weights_d) + + # take the 30% of lowest weight particles and distribute them uniformly. + if RANDOM_PARTICLE_RATIO > 0: + n_uniform = int(round(self.n_particles * RANDOM_PARTICLE_RATIO)) + indices = np.argsort(self.weights_a) + self.states[indices[:n_uniform], 0] = np.random.choice(self.angles_deg, n_uniform) + indices = np.argsort(self.weights_d) + self.states[indices[:n_uniform], 1] = np.random.choice(self.distances_cm, n_uniform) + + # more robust if we always resample. + if True: #self.effective_n() < self.states.shape[0] / 2: + self.state = State.NEED_RESAMPLE + else: + self.state = State.READY + + def predict(self): + """Predict new particles based on movement estimates.""" + if not self.state == State.NEED_PREDICT: + return + + if (not self.predict_uniform) and self.filled: + # rolling window with two elements: + previous = 1 if self.index == 0 else 0 + pos_delta = self.positions[self.index] - self.positions[previous] + rot_delta = self.rotations[self.index] - self.rotations[previous] + + a_global = self.rotations[previous] + self.states[:, 0] + self.states[:, 0] = from_0_to_360(self.states[:, 0] - rot_delta) + self.states[:, 1] = self.states[:, 1] - get_normal_matrix(a_global) @ pos_delta + + self.states[:, 0] += np.random.normal( + scale=STD_ANGLE_DEG, size=self.n_particles + ) + self.states[:, 1] += np.random.normal( + scale=STD_DISTANCE_CM, size=self.n_particles + ) + + self.states[:, 1] = np.clip( + self.states[:, 1], + a_min=self.distances_cm[0], + a_max=self.distances_cm[-1], + ) + + self.state = State.NEED_UPDATE + + def estimate(self): + """Returns mean and variance of the weighted particles.""" + import scipy.stats + + if not self.state == State.READY: + print("Warning: not ready for estimation yet.") + return None + + mean_distance = np.average(self.states[:, 1], weights=self.weights_d) + var_distance = np.average( + (self.states[:, 1] - mean_distance) ** 2, weights=self.weights_d + ) + + sin_ = np.sum( + np.multiply(np.sin(self.states[:, 0] / 180 * np.pi), self.weights_a) + ) + cos_ = np.sum( + np.multiply(np.cos(self.states[:, 0] / 180 * np.pi), self.weights_a) + ) + mean_angle = 180 / np.pi * np.arctan2(sin_, cos_) + mean_angle %= 360 + var_angle = 180 / np.pi * np.sqrt(sin_**2 + cos_**2) + return (mean_distance, mean_angle), (var_distance, var_angle) + + def resample(self): + if not self.state == State.NEED_RESAMPLE: + return + + indices_a = resample(self.weights_a) + resample_from_index(self.states[:, 0], self.weights_a, indices_a) + + indices_d = resample(self.weights_d) + resample_from_index(self.states[:, 1], self.weights_d, indices_d) + self.state = State.READY diff --git a/src/audio_gtsam/audio_gtsam/wall_backend.py b/src/audio_gtsam/audio_gtsam/wall_backend.py index fb352558..fa26eecc 100644 --- a/src/audio_gtsam/audio_gtsam/wall_backend.py +++ b/src/audio_gtsam/audio_gtsam/wall_backend.py @@ -340,7 +340,7 @@ def add_planes_from_distributions( distance = distance_estimates[d_i] azimuth = angle_estimates[a_i] distance_std = distance_stds[d_i] - azimuth_stds = angle_stds[a_i] + azimuth_std = angle_stds[a_i] if (distance is None) or (azimuth is None): continue @@ -359,7 +359,7 @@ def add_planes_from_distributions( # hack to get the local normal vector # TODO(FD) in add_plane, we will convert the angle back to normal, so should # probably refactor this. Also, when we don't care about the distance it - # is proably much easier to get the normal vector. + # is proably much easier to get the normal vector. plane_global = gtsam.OrientedPlane3( np.r_[normal_vector_global, 0, 10] ) # 10 is distance, doesn't matter @@ -424,12 +424,13 @@ def add_plane_from_distances( return normal_vector_global = -v_estimate_global / np.linalg.norm(v_estimate_global) + # hack to get the local normal vector # TODO(FD) in add_plane, we will convert the angle back to normal, so should # probably refactor this. Also, when we don't care about the distance it # is proably much easier to get the normal vector. plane_global = gtsam.OrientedPlane3( - np.r_[normal_vector_global, 0, 10] + np.r_[normal_vector_global, 0, 10] ) # 10 is distance, doesn't matter current_pose = self.result.atPose3(X(self.pose_index)) diff --git a/src/audio_stack/audio_stack/beam_former.py b/src/audio_stack/audio_stack/beam_former.py index 77af5a90..9943b3ef 100644 --- a/src/audio_stack/audio_stack/beam_former.py +++ b/src/audio_stack/audio_stack/beam_former.py @@ -15,8 +15,10 @@ current_dir = os.path.dirname(os.path.abspath(__file__)) sys.path.append(current_dir + "/../../../crazyflie-audio/python/") -from algos_beamforming import get_lcmv_beamformer_fast, get_das_beamformer, get_powers +from algos_beamforming import get_das_beamformer, get_powers +from algos_beamforming import get_lcmv_beamformer_fast as get_lcmv_beamformer +# from algos_beamforming import get_lcmv_beamformer as get_lcmv_beamformer LAMDA = 1e-5 # 1e-10 INVERSE = "pinv" # use standard pseudoinverse @@ -24,7 +26,7 @@ def normalize_rows(matrix, method): - """ Normalizes last dimension of matrix (can be more than 2-dimensional) """ + """Normalizes last dimension of matrix (can be more than 2-dimensional)""" if np.all(np.isnan(matrix)): print("Warning: not normalizeing, all nan.") return matrix @@ -106,21 +108,87 @@ def __init__(self, mic_positions=None): self.theta_scan = BeamFormer.theta_scan self.params = {} + def beamform_lcmv( + self, + R, + theta, + frequencies_hz, + mic_positions=None, + lamda=LAMDA, + inverse=INVERSE, + extra_constraints=[], + cancel_centre=False, + ): + if mic_positions is None: + mic_positions = self.mic_positions + constraints = extra_constraints + [(theta, 1)] + H_lcmv = get_lcmv_beamformer( + R, + frequencies_hz, + mic_positions, + constraints, + lamda=lamda, + inverse=inverse, + cancel_centre=cancel_centre, + ) + return H_lcmv + + def get_lcmv_spectrum( + self, + R, + frequencies_hz, + mic_positions=None, + lamda=LAMDA, + inverse=INVERSE, + extra_constraints=[], + cancel_centre=False, + ): + """Get LCMV spatial spectrum. + + :param R: autocorrelation tensor (n_frequencies x n_mics x n_mics) + :param frequencies_hz: list of frequencies (in Hz) + :param extra_constraints: list of additional constraints to be added to + LCMV (d(theta)*w=1 is always added). Given in the form [angle, value]. + + :return: spectrum of shape (n_frequencies x n_angles) + """ + if mic_positions is None: + mic_positions = self.mic_positions + spectrum = np.empty((len(frequencies_hz), len(self.theta_scan))) + for i, theta in enumerate(self.theta_scan): + H_mvdr = self.beamform_lcmv( + R, + theta, + frequencies_hz, + mic_positions, + lamda, + inverse, + extra_constraints=extra_constraints, + cancel_centre=cancel_centre, + ) + spectrum[:, i] = get_powers(H_mvdr, R) + return spectrum + def beamform_mvdr( self, R, theta, frequencies_hz, mic_positions=None, lamda=LAMDA, inverse=INVERSE ): if mic_positions is None: mic_positions = self.mic_positions constraints = [(theta, 1)] - H_mvdr = get_lcmv_beamformer_fast( - R, frequencies_hz, mic_positions, constraints, lamda=lamda, inverse=inverse, + H_mvdr = get_lcmv_beamformer( + R, + frequencies_hz, + mic_positions, + constraints, + lamda=lamda, + inverse=inverse, ) return H_mvdr def get_mvdr_spectrum( self, R, frequencies_hz, mic_positions=None, lamda=LAMDA, inverse=INVERSE ): - """ Get MVDR spatial spectrum. + """Get MVDR spatial spectrum. :param R: autocorrelation tensor (n_frequencies x n_mics x n_mics) :param frequencies_hz: list of frequencies (in Hz) @@ -134,7 +202,6 @@ def get_mvdr_spectrum( H_mvdr = self.beamform_mvdr( R, theta, frequencies_hz, mic_positions, lamda, inverse ) - spectrum[:, i] = get_powers(H_mvdr, R) return spectrum @@ -143,22 +210,27 @@ def beamform_das(self, theta, frequencies_hz, mic_positions=None): mic_positions = self.mic_positions return get_das_beamformer(theta, frequencies_hz, mic_positions) - def get_das_spectrum(self, R, frequencies, mic_positions=None): - """ Get DAS spatial spectrum. + def get_das_spectrum(self, R, frequencies_hz, mic_positions=None, phat=False): + """Get DAS spatial spectrum. - see get_mvdr_spectrum for parameters. + :param phat: use phase transform, i.e. divide R by its magnitude. + see get_mvdr_spectrum for other parameters. """ if mic_positions is None: mic_positions = self.mic_positions - spectrum = np.empty((len(frequencies), len(self.theta_scan))) + spectrum = np.empty((len(frequencies_hz), len(self.theta_scan))) for i, theta in enumerate(self.theta_scan): - H_das = self.beamform_das(theta, frequencies, mic_positions) - spectrum[:, i] = get_powers(H_das, R) + H_das = self.beamform_das(theta, frequencies_hz, mic_positions) + + if phat: + spectrum[:, i] = get_powers(H_das, R / np.abs(R)) + else: + spectrum[:, i] = get_powers(H_das, R) return spectrum def get_correlation(self, signals_f): - """ Get autocorrelation tensor. + """Get autocorrelation tensor. :param signals_f: frequency response (n_frequencies x n_mics) """ if signals_f.shape[0] < signals_f.shape[1]: @@ -172,7 +244,7 @@ def get_correlation(self, signals_f): ) def shift_spectrum(self, spectrum, delta_deg): - """ shift spectrum by delta_deg. + """shift spectrum by delta_deg. :param spectrum: spatial spectrum (n_frequencies x n_angles) :param delta_deg: by how many angles to shfit the spectrum @@ -206,7 +278,7 @@ def init_dynamic_estimate( ) def add_to_dynamic_estimates(self, spectrum, orientation_deg=0): - """ Add new spectrum to list and remove outdated ones. + """Add new spectrum to list and remove outdated ones. :param spectrum: spatial spectrum of shape (n_frequencies, n_angles) :param orientation_deg: drone orientation_deg in degrees @@ -224,7 +296,7 @@ def add_to_dynamic_estimates(self, spectrum, orientation_deg=0): def add_signals_to_dynamic_estimates( self, signals_f, frequencies, orientation_deg=0, method="das" ): - """ Add new spectrum to list and remove outdated ones. + """Add new spectrum to list and remove outdated ones. :param orientation_deg: drone orientation_deg in degrees """ @@ -236,7 +308,7 @@ def add_signals_to_dynamic_estimates( return self.add_to_dynamic_estimates(spectrum, orientation_deg) def get_dynamic_estimate(self): - """ Get current estimate + """Get current estimate :return: spectrum estimate of shape (n_angles,) """ @@ -263,7 +335,9 @@ def init_multi_estimate(self, frequencies, combination_n): np.nan, ) self.index_multi = 0 - self.params["multi"] = dict(combination_n=combination_n,) + self.params["multi"] = dict( + combination_n=combination_n, + ) def add_to_multi_estimate( self, signals_f, frequencies, time_sec, orientation_deg, offset=0, position=None @@ -319,7 +393,7 @@ def get_multi_R(self): return self.get_correlation(signals_f_aligned) def get_multi_estimate(self, method="mvdr", lamda=LAMDA): - """ Get current estimate + """Get current estimate :return: spectrum of shape (n_frequencies x n_angles) """ diff --git a/src/crazyflie_demo/crazyflie_demo/wall_detection.py b/src/crazyflie_demo/crazyflie_demo/wall_detection.py index 629d7c17..8076b2b5 100644 --- a/src/crazyflie_demo/crazyflie_demo/wall_detection.py +++ b/src/crazyflie_demo/crazyflie_demo/wall_detection.py @@ -24,9 +24,12 @@ sys.path.append("python/") from utils.data_collector import DataCollector from utils.inference import Inference, get_approach_angle_fft +from utils.inference import get_angle_distribution as get_beamform_distribution + from utils.estimators import DistanceEstimator, AngleEstimator from utils.moving_estimators import MovingEstimator from utils.particle_estimators import ParticleEstimator +from utils.histogram_estimators import HistogramEstimator # dslice FREQ = 3000 # mono frequency signal @@ -37,7 +40,7 @@ # corresponds to discretization "fine": DISTANCES_CM = np.arange(7, 80, step=2) -ANGLES_DEG = np.arange(360, step=10) +ANGLES_DEG = np.arange(360, step=1) WALL_ANGLE_DEG = 90 # for raw distribution only N_MICS = 4 @@ -51,7 +54,6 @@ MAG_THRESH = 1e-3 - class State(Enum): GROUND = 0 HOVER = 1 @@ -111,7 +113,7 @@ def get_angle_distribution(dslices, resolve_side=False): class PythonLogger(object): - """ Simple wrapper for ROS2 logger that doesn't require ROS """ + """Simple wrapper for ROS2 logger that doesn't require ROS""" def warn(self, msg): print("Warn:", msg) @@ -126,12 +128,15 @@ def error(self, msg): class WallDetection(NodeWithParams): PARAMS_DICT = {"mode": Mode.FSLICE.value, "drone": int(DRONE)} + BEAMFORM = False + MASK_BAD = "fixed" # MASK_BAD = "adaptive" # MASK_BAD = None BAD_FREQ_RANGES = [[0, 3100], [3600, 3800]] # removes two frequencies - OUTLIER_FACTOR = 10 # reject values outside of OUTLIER_FACTOR * std window + BAD_FREQ_RANGES_EPUCK = [[0, 1000], [5300, 6000]] # removes two frequencies + OUTLIER_FACTOR = None # reject values outside of OUTLIER_FACTOR * std window # need at least two, otherwise end up with all-ones magnitudes_calib. N_CALIBRATION = 2 @@ -143,14 +148,16 @@ class WallDetection(NodeWithParams): # N_CALIBRATION = 10 # CALIBRATION= "fixed" - SIMPLIFY_ANGLES = True + # if set to true, we use simplify the angles, meaning that we use + # the forward direction as our angle estimate. If we do not simplify + # angles, we simply use a uniform prior on the angle estimate. + SIMPLIFY_ANGLES = False # estimator variables N_WINDOW = 5 - RELATIVE_MOVEMENT_STD = 1.0 - N_PARTICLES = 100 + N_PARTICLES = 400 - def __init__(self, python_only=False, estimator="moving"): + def __init__(self, python_only=False, estimator="moving", angles_deg=ANGLES_DEG): if not python_only: print("initializing ros stuff") super().__init__("wall_detection") @@ -172,7 +179,10 @@ def __init__(self, python_only=False, estimator="moving"): SignalsFreq, "audio/signals_f", self.signals_synch.listener_callback, 10 ) self.subscription_pose = self.create_subscription( - PoseRaw, "geometry/pose_raw_synch", self.listener_callback, 10, + PoseRaw, + "geometry/pose_raw_synch", + self.listener_callback, + 10, ) if PUBLISH_RAW: @@ -198,19 +208,18 @@ def __init__(self, python_only=False, estimator="moving"): self.estimator = MovingEstimator( n_window=self.N_WINDOW, distances_cm=DISTANCES_CM, - angles_deg=ANGLES_DEG, - relative_movement_std=self.RELATIVE_MOVEMENT_STD, + angles_deg=angles_deg, + ) + elif estimator == "histogram": + self.estimator = HistogramEstimator( + distances_cm=DISTANCES_CM, + angles_deg=angles_deg, ) elif estimator == "particle": - ParticleEstimator.ANGLES_DEG = ANGLES_DEG - ParticleEstimator.DISTANCE_RANGE_CM = [ - np.min(DISTANCES_CM), - np.max(DISTANCES_CM), - ] self.estimator = ParticleEstimator( - n_particles=self.N_PARTICLES, global_=False + n_particles=self.N_PARTICLES, ) - elif type(estimator) in [MovingEstimator, ParticleEstimator]: + elif type(estimator) in [MovingEstimator, ParticleEstimator, HistogramEstimator]: self.estimator = estimator else: raise ValueError(estimator) @@ -242,10 +251,10 @@ def __init__(self, python_only=False, estimator="moving"): self.start_forward = None def flight_check(self, position_cm=None): - """ + """ drone=0: analyzing bag file, should be "flying" to do analysis drone=1: only buzzer, for debugging, don't mind if not flying - drone=2: full experiment, has to be flying. + drone=2: full experiment, has to be flying. """ if self.current_params["drone"] == 1: return True @@ -290,9 +299,9 @@ def add_to_calib_data(self, magnitudes, verbose=False): def add_to_calib_iir(self, magnitudes): if self.calibration is None: self.calibration = magnitudes - self.calibrationsq = magnitudes ** 2 + self.calibrationsq = magnitudes**2 else: - valid = ~np.isnan(self.calibration) + valid = ~np.isnan(self.calibration) self.calibration[valid] = (1 - self.ALPHA_IIR) * self.calibration[ valid ] + self.ALPHA_IIR * magnitudes[valid] @@ -305,7 +314,7 @@ def add_to_calib_iir(self, magnitudes): self.calibration_count += 1 def calibrate(self, magnitudes): - """ Note that invalid values are masked (set to zero). """ + """Note that invalid values are masked (set to zero).""" if self.CALIBRATION in ("fixed", "window"): if self.calibration is None: if self.calibration_data.shape[2] <= 1: @@ -339,7 +348,7 @@ def calibrate(self, magnitudes): ) return magnitudes - var = self.calibrationsq - self.calibration ** 2 + var = self.calibrationsq - self.calibration**2 self.calibration_std = np.full(var.shape, np.nan) self.calibration_std[var > 0] = np.sqrt(var[var > 0]) self.calibration_std[self.calibration_std == 0] = np.nan @@ -354,12 +363,16 @@ def calibrate(self, magnitudes): magnitudes_calib[~valid] = np.nan return magnitudes_calib - def get_raw_distributions(self, magnitudes_calib, freqs): + def get_raw_distributions(self, magnitudes_calib, freqs, chosen_mics=None): diff_dict = {} + if chosen_mics is None: + chosen_mics = range(magnitudes_calib.shape[0]) self.inf_machine.add_data( - magnitudes_calib, freqs, # 4 x 32 + magnitudes_calib, + freqs, # 4 x 32 + mics=chosen_mics ) - for mic_i in range(magnitudes_calib.shape[0]): + for mic_i in chosen_mics: __, prob_mic, diff = self.inf_machine.do_inference(ALGORITHM, mic_i) diff_dict[mic_i] = (diff, prob_mic) return diff_dict @@ -372,6 +385,12 @@ def mask_bad_measurements(self, magnitudes, freqs, verbose=False): remove |= (freq_range[0] <= freqs) & (freqs <= freq_range[1]) magnitudes[:, remove] = np.nan return magnitudes + elif self.MASK_BAD == "fixed_epuck": + remove = np.zeros(magnitudes.shape[1], dtype=bool) + for freq_range in self.BAD_FREQ_RANGES_EPUCK: + remove |= (freq_range[0] <= freqs) & (freqs <= freq_range[1]) + magnitudes[:, remove] = np.nan + return magnitudes elif self.MASK_BAD == "adaptive": if self.calibration is None: return magnitudes @@ -409,20 +428,21 @@ def mask_bad_measurements(self, magnitudes, freqs, verbose=False): raise ValueError(self.MASK_BAD) def listener_callback_offline( - self, signals_f, freqs, position_cm, yaw_deg, timestamp=0 + self, signals_f, freqs, position_cm, yaw_deg, timestamp=0, chosen_mics=None ): + if not self.flight_check(position_cm): - # print("did not pass flight check:", position_cm) return from audio_stack.parameters import WINDOW_CORRECTION - magnitudes = np.abs(signals_f).T # 4 x 20 + assert signals_f.shape[1] == len(freqs) + magnitudes = np.abs(signals_f) + # just a formality to compare bag file to csv file. magnitudes *= WINDOW_CORRECTION[2] magnitudes = self.mask_bad_measurements(magnitudes, freqs) - self.add_to_calib(magnitudes) magnitudes_calib = self.calibrate(deepcopy(magnitudes)) @@ -431,29 +451,66 @@ def listener_callback_offline( return try: - diff_dict = self.get_raw_distributions(magnitudes_calib, freqs) + diff_dict = self.get_raw_distributions(magnitudes_calib, freqs, chosen_mics) except Exception as e: print("could not process calibrated magnitudes:", e) - raise e - return None + raise + #print(magnitudes) + #print(magnitudes_calib, freqs, chosen_mics) + return + + return_dict = {} - distances_cm, probabilities = get_distance_distribution(diff_dict) + # get the distance probabilities without considering movement + dist_static, prob_dist_static = get_distance_distribution(diff_dict) + return_dict["dist_static"] = dist_static + return_dict["prob_dist_static"] = prob_dist_static + # get distance and angle probabilities, considering movement. + # diff_dict is in centimeters self.estimator.add_distributions( - diff_dict, position_cm=position_cm, rot_deg=yaw_deg, + diff_dict, + position_cm=position_cm, + rot_deg=yaw_deg, ) + if self.BEAMFORM: + if chosen_mics is not None: + mics = self.estimator.context.mics[chosen_mics, :].T + else: + mics = self.estimator.context.mics.T + + # TODO(FD): may need to rotate mics by current yaw? + #from audio_stack.beam_former import rotate_mics + #print("rotating by", 90) + #mics = rotate_mics(mics.T, 90).T + #print("-45 works!") + #print("mics for beamformer:", repr(mics)) + + #print("beamforming with", mics.shape, signals_f.shape) + angle_static, prob_angle_static = get_beamform_distribution( + signals_f.T, freqs, mics # 32 x 4, 32, 2 x 4 + ) + self.estimator.add_angle_distribution(angle_static, prob_angle_static) + return_dict["angle_static"] = angle_static + return_dict["prob_angle_static"] = prob_angle_static + ( - distances_cm, - probabilities_moving, - angles_deg, - probabilities_moving_angle, + dist_moving, + prob_dist_moving, + angle_moving, + prob_angle_moving, ) = self.estimator.get_distributions(simplify_angles=self.SIMPLIFY_ANGLES) - return ( - distances_cm, - probabilities, - probabilities_moving, - probabilities_moving_angle, - ) + return_dict["angle_moving"] = angle_moving + return_dict["prob_angle_moving"] = prob_angle_moving + return_dict["dist_moving"] = dist_moving + return_dict["prob_dist_moving"] = prob_dist_moving + return return_dict + # return ( + # dist_static, + # prob_dist_static, + # prob_dist_moving, + # prob_angle_moving, + # ) def listener_callback(self, msg_pose): if not self.state in [State.WAIT_DISTANCE, State.WAIT_ANGLE, State.WAIT_CALIB]: @@ -521,11 +578,15 @@ def listener_callback(self, msg_pose): if PUBLISH_MOVING: self.estimator.add_distributions( - diff_dict, position_cm=r_world * 1e2, rot_deg=yaw, - ) - (distances_cm, probabilities, *_,) = self.estimator.get_distributions( - simplify_angles=SIMPLIFY_ANGLES + diff_dict, + position_cm=r_world * 1e2, + rot_deg=yaw, ) + ( + distances_cm, + probabilities, + *_, + ) = self.estimator.get_distributions(simplify_angles=SIMPLIFY_ANGLES) # self.logger.warn(f"{timestamp}, after adding {r_world[0] * 1e2:.2f}, {yaw:.1f}: {probabilities[2]}") # TODO(FD) to save time, we could consider only publishing the distribution @@ -769,6 +830,18 @@ def get_result_wall(self, future): # state_machine self.logger.warn(f"Avoiding detected wall!") self.already_asking = False + def print_params(self): + print("calibration:", self.CALIBRATION) + print("n_calibration:", self.N_CALIBRATION) + print("alpha_iir:", self.ALPHA_IIR) + print("mask bad:", self.MASK_BAD ) + print("outlier factor:", self.OUTLIER_FACTOR ) + print("n_window:", self.N_WINDOW) + print("simplify_angles:", self.SIMPLIFY_ANGLES ) + print("beamform:", self.BEAMFORM) + print("bad freq ranges:", self.BAD_FREQ_RANGES) + print("bad freq ranges epuck:", self.BAD_FREQ_RANGES_EPUCK) + print("n_particles:", self.N_PARTICLES) def main(args=None): rclpy.init(args=args) diff --git a/src/crazyflie_description/crazyflie_description_py/experiments.py b/src/crazyflie_description/crazyflie_description_py/experiments.py index 3fc248ba..24f5128b 100644 --- a/src/crazyflie_description/crazyflie_description_py/experiments.py +++ b/src/crazyflie_description/crazyflie_description_py/experiments.py @@ -7,6 +7,7 @@ # corresponds to the setup in BC325 with stepper motor: WALL_ANGLE_DEG_STEPPER = 132 # angle of wall in degrees, at start of experiment WALL_DISTANCE_CM_STEPPER = 8 # distance of wall in centimeters, at start of experiment +DISTANCES_CM = np.arange(50, step=1)[::-1] + WALL_DISTANCE_CM_STEPPER # corresponds to flying experiments: WALL_DISTANCE_M = 0.2 # distance of wall in meters, at start of experiment diff --git a/src/epuck_description/epuck_description_py/experiments.py b/src/epuck_description/epuck_description_py/experiments.py index f231baa0..d2055077 100644 --- a/src/epuck_description/epuck_description_py/experiments.py +++ b/src/epuck_description/epuck_description_py/experiments.py @@ -5,4 +5,4 @@ # corresponds to the setup in BC325 on the floor WALL_ANGLE_DEG_STEPPER = 270 # angle of wall in degrees, at start of experiment -DISTANCES_CM = np.linspace(17.6, 60, 30) # range of distances travelled in wall detection experiment +DISTANCES_CM = np.linspace(17.6, 60, 30)[::-1] # range of distances travelled in wall detection experiment