Skip to content

Commit

Permalink
finished first version of localization notebook
Browse files Browse the repository at this point in the history
  • Loading branch information
@gkueppers
gkueppers committed Oct 16, 2023
1 parent 638c9b1 commit 2ee4f3b
Showing 1 changed file with 119 additions and 59 deletions.
178 changes: 119 additions & 59 deletions section_2_sensor_data_processing/8_localization.ipynb
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,7 @@
"In this assignment, we will use the bag-file that has been recorded in the end of the C++ task to evaluate the implemented solution.\n",
"There to we will walk through the following steps:\n",
"\n",
"- Importing and parsing of the information from the bag-file using [rosbags](https://pypi.org/project/rosbags/)\n",
"- Importing and parsing of the information from the bag-file using [rosbags](https://pypi.org/project/rosbags/).\n",
"- Converting the imported data into a [pandas](https://pandas.pydata.org/) dataframe.\n",
"- Assignment of ground-truth poses to the corresponding estimated vehicle poses\n",
"- Implementation and visualization of various metrics for the evaluation of the pose estimation."
Expand Down Expand Up @@ -376,20 +376,15 @@
"cell_type": "markdown",
"metadata": {},
"source": [
"## Task: Calculation of metrics for the evaluation of localization quality"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"We will start with the calculation of the most basic metrics: The distance error between estimate and ground truth in `x`, `y` and `psi`.\n",
"## Task: Calculation and investigation of metrics for the evaluation of localization quality\n",
"\n",
"We will start with the calculation of the most basic metrics: The distance error between ground truth and estimate in `x`, `y` and `psi`.\n",
"\n",
"Pandas allows us to add new fields to the data frame based on calculations in a very intuitive way. Use the example below to add the corresponding columns named `dx`, `dy` and `dpsi` to the data frame.\n",
"\n",
"#### Example:\n",
"`example_data_frame['C'] = example_data_frame['A'] + example_data_frame['B']`\n",
"- In this example `A` and `B` are existing fields in the data frame, the new field with name `C` will be added."
"- In this example `A` and `B` are existing fields in the data frame, the new field with name `C` will be added and represents the sum of `A` and `B`."
]
},
{
Expand All @@ -409,31 +404,90 @@
"### END CODE HERE ###"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Now that we have calculated the deviations in x-, y- and psi, we can examine them in more detail. Obviously, we could plot the values over time again, but the information we would get from this is relatively similar to the 2D plot we looked at before where these absolute deviations are visually represented. Alternatively, we will plot the deviations in the form of boxplots.\n",
"\n",
"Execute the code cell below to generate boxplots for `dx`, `dy` and `dpsi`."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"fig, ax = plt.subplots(1,3,figsize=(20, 4))\n",
"df.boxplot(column='dx', ax=ax[0])\n",
"df.boxplot(column='dy', ax=ax[1])\n",
"df.boxplot(column='dpsi', ax=ax[2])\n",
"df.boxplot(column='dx', ax=ax[0], whis=[0, 100])\n",
"df.boxplot(column='dy', ax=ax[1], whis=[0, 100])\n",
"df.boxplot(column='dpsi', ax=ax[2], whis=[0, 100])\n",
"ax[0].set_ylim(-2.5, 2.5)\n",
"ax[1].set_ylim(-0.15, 0.15)\n",
"ax[2].set_ylim(-0.1, 0.1)\n",
"\n",
"plt.show()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The box in this plot represents the range of values in which 50% of the data fall. The lower boundary of the box is also called the lower quartile and describes the value that is greater than or equal to 25% of the smallest values in the data set. The upper quartile corresponds to the value that is greater or equal than 75% of the smallest values. In other words, 25% of all data points are greater than this value.\n",
"\n",
"Inside the box, there is a green line that describes the median. The median divides the data set into the largest and smallest values, i.e. 50% of all values are above and 50% of all values are below the median.\n",
"\n",
"In addition to the box and the median, the so-called whiskers are also visualized. In this case, the whisker describes the minimum or maximum value in the data set. Note that depending on the axis scaling, not all whiskers can be recognized.\n",
"\n",
"The interpretation of `dx` and `dy` is not very meaningful from the technical point of view as they indicate the deviations of the position in the map frame. Related to the vehicle, the evaluation in longitudinal and lateral direction is of particular importance. The longitudinal deviation describes the deviation along the longitudinal axis of the vehicle, and the lateral deviation describes the deviation perpendicular to the longitudinal axis of the vehicle.\n",
"\n",
"We will determine these vehicle centered translational deviations in the following. Before we do so, we can briefly discuss the boxplot for the yaw angle deviation (`dpsi`). The median deviation of the yaw angle is just under a quarter of a degree. It is noticeable that the upper quartile is greater in magnitude than the lower quartile. In other words, there is a greater deviation to the left than to the right. A possible explanation for this may be that the vehicle makes two right-hand turns, resulting in deviations in the vehicle yaw angle to the left. We have already explained the large minimum and maximum values in the above plot of the deviation of `psi` over time. We came to the conclusion that the estimation of the yaw angle from two sequential gnss poses is mainly responsible for this. "
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Now lets calculate the longitudinal and lateral deviation relative to the `ground_truth` pose of the vehicle. Since `dx` and `dy` are given with respect to the map-frame we need to rotate these values with respect of the yaw angle (`psi_ground_truth`). The transformation can be implemented using the following equations:\n",
"\n",
"$d_{lon} = dx * cos(\\psi) + dy * sin(\\psi)$\n",
"\n",
"$d_{lat} = -dx * sin(\\psi) + dy * cos(\\psi)$\n",
"\n",
"You can apply numpy's trigonometric functions on coloums of your pandas data-frame:\n",
"#### Example:\n",
"`example_data_frame['sin_x'] = np.sin(example_data_frame['x'])`\n",
"- In this example `x` is an existing field in the data frame. The new field with name `sin_x` will be added representing $sin(x)$ for each value of `x` in the data-frame.\n",
"\n",
"Now complete the cell below to calculate and add `dlon` and `dlat` to the data-frame `df`.\n",
"\n",
"##### __Hints__:\n",
"- `psi_ground_truth` is stored in degrees within the data-frame while `np.sin` expects the input to be radians\n",
"- The constant $\\pi$ can be accessed through `np.pi` "
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"fig, ax = plt.subplots(1,2,figsize=(20, 4))\n",
"ax[0].set_ylim(-1, 1)\n",
"ax[1].set_ylim(-1, 1)\n",
"df.plot(x='t', y='dx', ax=ax[0])\n",
"df.plot(x='t', y='dy', ax=ax[1])\n",
"plt.show()"
"### START CODE HERE ###\n",
"\n",
"\n",
"\n",
"### Solution\n",
"df['dlon'] = df['dx']*np.cos(df['psi_ground_truth']*np.pi/180.0) + df['dy']*np.sin(df['psi_ground_truth']*np.pi/180.0)\n",
"df['dlat'] = -df['dx']*np.sin(df['psi_ground_truth']*np.pi/180.0) + df['dy']*np.cos(df['psi_ground_truth']*np.pi/180.0)\n",
"### END CODE HERE ###"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Now that we have calculated `dlon` and `dlat` we can visualize them again in the form of a boxplot. Execute the code cell below to generate the plots."
]
},
{
Expand All @@ -442,18 +496,33 @@
"metadata": {},
"outputs": [],
"source": [
"fig, ax = plt.subplots(1,1,figsize=(20, 4))\n",
"ax.set_ylim(-15, 15)\n",
"df.plot(x='t', y='dpsi', ax=ax)\n",
"fig, ax = plt.subplots(1,3,figsize=(20, 4))\n",
"df.boxplot(column='dlon', ax=ax[0], whis=[0, 100])\n",
"df.boxplot(column='dlat', ax=ax[1], whis=[0, 100])\n",
"df.boxplot(column='dlat', ax=ax[2], whis=[0, 100])\n",
"ax[1].set_ylim(-0.25, 0.25)\n",
"plt.show()"
]
},
{
"cell_type": "code",
"execution_count": null,
"cell_type": "markdown",
"metadata": {},
"outputs": [],
"source": []
"source": [
"The left graph shows the boxplot for `dlon` while the other two graphs show the boxplot for `dlat`. In each case only the y-axis is scaled differently in order to better recognize the box and the minimum and maximum values.\n",
"\n",
"You can see that the error in the lateral direction is distributed on significantly smaller scales than in the longitudinal direction. This may be due to the fact that the vehicle performs only few maneuvers, which challenge the localization in the context of the lateral direction.\n",
"\n",
"If you compare the boxplots of `dlon` and `dlat` with those of `dx` and `dy`, you can see a certain correlation. This is mainly due to the fact in the scenario we investigated, the vehicle moves in the direction of the x-axis of the map coordinate system for a quite long time, which results in a certain correlation. In reality, however, this is usually not the case, which is why it makes sense to evaluate in the lateral and longitudinal directions."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Finally, we investigate to how the lateral and longitudinal errors are distributed spatially. For this purpose, the ground-trouth trajectory is visualized in a 2D plot and the longitudinal and lateral errors are represented by color coding of the points.\n",
"\n",
"Execute the code cells below to generate the plots."
]
},
{
"cell_type": "code",
Expand All @@ -462,41 +531,22 @@
"outputs": [],
"source": [
"colormap = plt.get_cmap('viridis')\n",
"normalize = Normalize(vmin=0.0, vmax=5.0)\n",
"normalize = Normalize(vmin=0, vmax=2.0)\n",
"scalar_mappable = ScalarMappable(cmap=colormap, norm=normalize)\n",
"colors = scalar_mappable.to_rgba(np.abs(df['dpsi']))\n",
"colors = scalar_mappable.to_rgba(np.abs(df['dlon']))\n",
"fig, ax = plt.subplots(1,1,figsize=(20, 8))\n",
"ax.axis('equal')\n",
"plt.scatter(df['x_ground_truth'], df['y_ground_truth'], c=colors, marker='.')\n",
"colorbar = plt.colorbar(scalar_mappable)\n",
"colorbar.set_label('dpsi')\n",
"plt.show()"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"fig, ax = plt.subplots(1,2,figsize=(20, 4))\n",
"ax[0].set_ylim(-1, 1)\n",
"ax[1].set_ylim(-1, 1)\n",
"combined_df.plot(x='t', y='dlon', ax=ax[0])\n",
"combined_df.plot(x='t', y='dlat', ax=ax[1])\n",
"colorbar.set_label('dlon')\n",
"plt.show()"
]
},
{
"cell_type": "code",
"execution_count": null,
"cell_type": "markdown",
"metadata": {},
"outputs": [],
"source": [
"fig, ax = plt.subplots(1,2,figsize=(20, 4))\n",
"combined_df.boxplot(column='dlon', ax=ax[0])\n",
"combined_df.boxplot(column='dlat', ax=ax[1])\n",
"plt.show()"
"The plot above shows how the longitudinal error is distributed. It becomes visible that the error increases especially in the last section because of the failing LiDAR odometry. It is also interesting that in the middle section, where the vehicle moves in the y-direction of the map, larger errors occur. This is possibly due to the speed of the vehicle in this section, which can be seen from the larger distances between the points."
]
},
{
Expand All @@ -505,29 +555,39 @@
"metadata": {},
"outputs": [],
"source": [
"\n",
"colormap = plt.get_cmap('viridis')\n",
"normalize = Normalize(vmin=0.0, vmax=5.0)\n",
"normalize = Normalize(vmin=0, vmax=0.2)\n",
"scalar_mappable = ScalarMappable(cmap=colormap, norm=normalize)\n",
"colors = scalar_mappable.to_rgba(np.abs(combined_df['dpsi']))\n",
"colors = scalar_mappable.to_rgba(np.abs(df['dlat']))\n",
"fig, ax = plt.subplots(1,1,figsize=(20, 8))\n",
"ax.axis('equal')\n",
"plt.scatter(combined_df['x_ground_truth'], combined_df['y_ground_truth'], c=colors)\n",
"plt.scatter(df['x_ground_truth'], df['y_ground_truth'], c=colors, marker='.')\n",
"colorbar = plt.colorbar(scalar_mappable)\n",
"colorbar.set_label('dpsi')\n",
"colorbar.set_label('lat')\n",
"plt.show()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The lateral error is especially increased in the areas where the vehicle passes through a curve. As already mentioned, this is due to the estimation of the vehicle yaw angle."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Wrap Up\n",
"\n",
"- You learned how to apply...\n",
"- You learned how to apply...\n",
"- You learned how to...\n",
"- You learned how to..."
"Congratulations, you have successfully completed the Notebook Exercise regarding the section of vehicle localization!\n",
"\n",
"In this task you learned how to ...\n",
"\n",
"- ... import and parse information from a ROS 2 bag-file using [rosbags](https://pypi.org/project/rosbags/).\n",
"- ... convert the imported data into a [pandas](https://pandas.pydata.org/) dataframe.\n",
"- ... assign the ground-truth poses to the corresponding vehicle poses estimated by the implemented localization stack.\n",
"- ... implement and display various metrics for the evaluation of the pose estimation."
]
},
{
Expand Down

0 comments on commit 2ee4f3b

Please sign in to comment.