diff --git a/section_2_sensor_data_processing/8_localization.ipynb b/section_2_sensor_data_processing/8_localization.ipynb index b35300e..37f2e9f 100644 --- a/section_2_sensor_data_processing/8_localization.ipynb +++ b/section_2_sensor_data_processing/8_localization.ipynb @@ -20,12 +20,12 @@ "source": [ "Welcome to the assignment **Localization**.\n", "\n", - "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", + "In this assignment, we will use the bag file that has been recorded at the end of the C++ task to evaluate the implemented solution.\n", + "In particular, 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", + "- 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." ] }, @@ -58,7 +58,7 @@ "metadata": {}, "source": [ "## Introduction to `TrajectoryPoint2D`-Class\n", - "In this assignment we will evaluate the estimated pose of the vehicle. In the first step we will load the data from the bag file and store the information in a list of `TrajectoryPoint2D`.\n", + "In this assignment, we will evaluate the estimated pose of the vehicle. In the first step, we will load the data from the bag file and store the information in a list of `TrajectoryPoint2D`.\n", "\n", "An object of class `TrajectoryPoint2D` holds the following members:\n", " - `x` x-Position of the vehicle in map-frame [m]\n", @@ -70,9 +70,9 @@ " - `def from_odometry(self, odometry)`\n", " - `def from_pose(self, pose)`\n", " \n", - "Both functions are called on an `TrajectoryPoint2D`-object and set all member variables of this object based on a `nav_msgs::msg::Odometry` respectively a `geometry_msgs::msg::PoseStamped` message.\n", + "Both functions are called on an `TrajectoryPoint2D` object and set all member variables of this object based on a `nav_msgs::msg::Odometry` respectively a `geometry_msgs::msg::PoseStamped` message.\n", "\n", - "To make use of this class we import `TrajectoryPoint2D` in the following:" + "To make use of this class, we import `TrajectoryPoint2D` in the following:" ] }, { @@ -91,10 +91,10 @@ "## Task: Importing data from a ROS2 Bag-File using [rosbags](https://pypi.org/project/rosbags/)\n", "\n", "First of all we need to import the data that we've captured within a ROS2 bag-file.\n", - "We will use the [rosbags](https://pypi.org/project/rosbags/)-package for this purpose. For further information please refere to the [rosbags documentation](https://ternaris.gitlab.io/rosbags/).\n", + "We will use the [rosbags](https://pypi.org/project/rosbags/) package for this purpose. For further information please refere to the [rosbags documentation](https://ternaris.gitlab.io/rosbags/).\n", "\n", - "To load the bag-file, copy it into the `${REPOSITORY}/bag` directory of this repository.\n", - "Afterwards specify the name of the folder that contains the `*.db3`-file." + "To load the bag file, copy it into the `${REPOSITORY}/bag` directory of this repository.\n", + "Afterwards, specify the name of the folder that contains the `*.db3`-file." ] }, { @@ -116,10 +116,10 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "Now we need to finish the implementation of the `read_trajectory` function. This function reads a given bag-file and returns a list of `TrajectoryPoint2D` based on a `nav_msgs::msg::Odometry` or a `geometry_msgs::msg::PoseStamped` message. Try to understand how this function works and fill in the gaps.\n", + "Now we need to finish the implementation of the `read_trajectory` function. This function reads a given bag file and returns a list of `TrajectoryPoint2D` based on a `nav_msgs::msg::Odometry` or a `geometry_msgs::msg::PoseStamped` message. Try to understand how this function works and fill in the gaps.\n", "\n", "\n", - "Your task is to call the functions `from_odometry` respectively `from_point` on a `TrajectoryPoint2D`-object and append the resulting point to the trajectory." + "Your task is to call the functions `from_odometry` respectively `from_point` on a `TrajectoryPoint2D` object and append the resulting point to the trajectory." ] }, { @@ -130,7 +130,7 @@ "source": [ "def read_trajectory(bag_path, topic_name):\n", " \"\"\"\n", - " Reads a given bag-file and returns a list of TrajectoryPoint2D based on a nav_msgs::msg::Odometry or a geometry_msgs::msg::PoseStamped message\n", + " Reads a given bag file and returns a list of TrajectoryPoint2D based on a nav_msgs::msg::Odometry or a geometry_msgs::msg::PoseStamped message\n", " \n", " Arguments:\n", " bag_path -- String indicating the location of the bag-file\n", @@ -210,13 +210,13 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "## Task: Creating a pandas data frame from both trajectories\n", + "## Task: Creating a pandas DataFrame from both trajectories\n", "\n", - "Now that we've succesfully imported the trajectory-data, we will convert these into a [pandas data frame](https://pandas.pydata.org/docs/reference/api/pandas.DataFrame.html).\n", + "Now that we've succesfully imported the trajectory data, we will convert these into a [pandas DataFrame](https://pandas.pydata.org/docs/reference/api/pandas.DataFrame.html).\n", "\n", - "At first we will create two data frames: one for the ground-truth trajectory and one for the estimated trajectory. Each data frame contains four colums `t`, `x`, `y` and `psi`. In this case we use list comprehension to store all these trajectory-variables into the data frame.\n", + "At first, we will create two DataFrame: one for the ground truth trajectory and one for the estimated trajectory. Each DataFrame contains four colums `t`, `x`, `y` and `psi`. In our case, we use list comprehension to store all these trajectory variables into the DataFrame.\n", "\n", - "Your task is to replace all `None` placeholders within the code-cell below.\n", + "Your task is to replace all `None` placeholders within the code cell below.\n", "##### __Hints__:\n", "- The `None` placeholders refer to the specific trajectory we would like to derive the information from." ] @@ -244,9 +244,9 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "Now we have two data-frames, but to compare the trajectories we need an order of individual ground-truth and estimated poses from both data-frames. Therefore we want to combine the two data-frames into one. The goal is to assign a corresponding ground-truth pose to each estimated pose. Since the ground truth trajectory was published with a higher frequency (20Hz) compared to the estimated trajectory (10Hz), the temporal error should be maximal half the period of the publication frequency of the ground truth pose.\n", + "Now, we have two data-frames, but to compare the trajectories we want the individual ground truth and estimated poses from both DataFrames in only one DataFrame. To achieve this, we combine the two DataFrames into one. The goal is to assign a corresponding ground truth pose to each estimated pose. Since the ground truth trajectory was published with a higher frequency (20Hz) compared to the estimated trajectory (10Hz), the temporal error should be maximal at half the period of the publication frequency of the ground truth pose.\n", "\n", - "To merge both data-frames we will use the function `mege_asof` from pandas. This function allows to merge two data frames based on a distance metric of a defined field. In this case we use the time difference between the poses as metric. Your task is to choose the `tolerance` for matching two poses. Replace the `None` placeholder in the code cell below.\n", + "To merge both DataFrames, we will use the function `merge_asof` from pandas. This function allows to merge two DataFrames based on a distance metric of a defined field. In our case, we use the time difference between the poses as a metric. Your task is to choose the `tolerance` for matching two poses. Replace the `None` placeholder in the code cell below.\n", "##### __Hints__:\n", "- The ground truth pose is published with a frequency of 20 Hz. The period between two messages is therefore 0.05 seconds." ] @@ -319,20 +319,20 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "At first glance, the comparison of the trajectories in the overview view looks good. Only in the curves larger deviations are observable. These deviations are mainly due to the estimation of the vehicle yaw angle from two subsequent GNSS positions.\n", - "However, a look at the detailed views also shows that there are poor results in the pose estimates when driving straight ahead (in the area of $y \\approx 650 m$). In this area, LiDAR odometry could not provide sufficient motion estimate due to insufficient features. You should take into account that this is only noticeable because of the visualization of the actual measuring points in the plot. It therefore makes sense to visualize the actual measuring points at least in addition to the connecting lines.\n", + "At first glance, the comparison of the trajectories in the overview view looks good. Only in the turns, larger deviations are observable. These deviations are mainly due to the estimation of the vehicle yaw angle from two subsequent GNSS positions.\n", + "However, a look at the detailed views also shows that there are poor results in the pose estimates when driving straight (in the area of $y \\approx 650 m$). In this area, LiDAR odometry could not provide sufficient motion estimates due to insufficient features. You should take into account that this is only noticeable because of the visualization of the actual measuring points in the plot. It therefore makes sense to visualize the actual measuring points at least in addition to the connecting lines.\n", "\n", "### Task: Visualizing the yaw-angle\n", "\n", "In the following, we will also take a brief look at the yaw angle, since this is not explicitly shown in the 2D plot.\n", - "There to fill in the gaps in the code-cell below.\n", + "To do so, please fill in the gaps in the code-cell below.\n", "\n", - "In this plot we want to plot the yaw angle over time. Two plots are to be displayed. On the one hand the ground-truth yaw angle and on the other hand the estimated yaw angle.\n", + "In this plot, we want to plot the yaw angle over time. Two plots are to be displayed. On the one hand the ground truth yaw angle and on the other hand the estimated yaw angle.\n", "\n", "##### __Hints__:\n", "- The best way to get inspired is to look at the code example where we have plotted the trajectories x over y: `df.plot(x='x_ground_truth', y='y_ground_truth', ax=ax)`\n", - "- Time is denoted as `'t'` in the data frame\n", - "- The yaw angle is denoted as `'psi_'` in the data frame" + "- Time is denoted as `'t'` in the DataFrame\n", + "- The yaw angle is denoted as `'psi_'` in the DataFrame" ] }, { @@ -365,11 +365,11 @@ "tags": [] }, "source": [ - "The plot shows that the vehicle is initially aligned with a yaw angle of approx. 180°. Since the yaw angle is defined here between -180° and 180°, there are discontinuities that can be seen in the plot. However, these are not really dramatic in this case. It gets interesting at the point where the vehicle stops at a stop sign. Here, the estimated yaw angle jumps even though the vehicle is stationary. This behavior is also due to the estimation based on two sequential GNSS positions, since these are very close to each other, but can slightly deviate from each other due to measurement errors, resulting in arbitrary yaw angle estimates.\n", + "The plot shows that the vehicle is initially aligned with a yaw angle of approx. 180°. Since the yaw angle is defined between -180° and 180° here, there are discontinuities that can be seen in the plot. However, these are not really dramatic in this case. It gets interesting at the point where the vehicle stops at a stop sign. Here, the estimated yaw angle jumps even though the vehicle is stationary. This behavior is also due to the estimation based on two sequential GNSS positions, since these are very close to each other, but can slightly deviate from each other due to measurement errors, resulting in arbitrary yaw angle estimates.\n", "\n", - "With these simple analysis, we have already identified a major weakness of the implementation. This could be counteracted, for example, by using an additional sensor for direct measurement of the vehicle orientation. An example of such a sensor is a compass.\n", + "With this simple analysis, we have already identified a major weakness of the implementation. This could be counteracted, for example, by using an additional sensor for direct measurement of the vehicle orientation. An example of such a sensor is a compass.\n", "\n", - "In the following we will calculate some metrics to numerically describe the quality of the implemented approach." + "In the following, we will calculate some metrics to numerically describe the quality of the implemented approach." ] }, { @@ -380,11 +380,11 @@ "\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", + "Pandas allows us to add new fields to the DataFrame based on calculations in a very intuitive way. Use the example below to add the corresponding columns named `dx`, `dy` and `dpsi` to the DataFrame.\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 and represents the sum of `A` and `B`." + "- In this example `A` and `B` are existing fields in the DataFrame, the new field with name `C` will be added and represents the sum of `A` and `B`." ] }, { @@ -408,7 +408,7 @@ "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", + "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`." ] @@ -436,9 +436,9 @@ "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", + "Inside the box, there is a green line that describes the median. The median divides the data set into two classes containing the largest and the 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", + "In addition to the box and the median, the so-called whiskers are also visualized. In this case, the whiskers describe 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", @@ -449,21 +449,21 @@ "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", + "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", + "You can apply numpy's trigonometric functions on coloums of your pandas DataFrame:\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", + "- In this example `x` is an existing field in the DataFrame. 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", + "- `psi_ground_truth` is stored in degrees within the DataFrame while `np.sin` expects the input to be radians\n", "- The constant $\\pi$ can be accessed through `np.pi` " ] }, @@ -487,7 +487,7 @@ "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." + "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." ] }, { @@ -508,18 +508,18 @@ "cell_type": "markdown", "metadata": {}, "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", + "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." + "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 that in the scenario we investigated, the vehicle moves in the direction of the x-axis of the map coordinate system for quite a 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", + "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." ] @@ -571,7 +571,7 @@ "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." + "The lateral error is especially increased in the areas where the vehicle passes through a turn. As described before, this is due to the estimation of the vehicle yaw angle." ] }, { @@ -584,8 +584,8 @@ "\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", + "- ... 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." ]