Skip to content

Section 2 Localization

Jump to bottom
Guido Küppers edited this page Oct 5, 2023 · 9 revisions

Combining GNSS and LiDAR-Odometry for frequent pose estimation

In this workshop, we will perform localization based on GNSS and LiDAR-Measurements. To achieve a high frequent pose estimate we will fuse both sources of information.

We will use a recording from a simulation containing LiDAR-Pointclouds, GNSS Measurements and the corresponding ground-truth pose. To better understand the context of the environment, the recording also contains a camera image stream.

The learning goals of this workshop are ...

  • Setup a localization stack for automated driving in an urban environment using ROS 2
  • Utilize and investigate KISS-ICP LiDAR Odometry
  • Implement further processing of the lidar odometry output by combining it with low-frequent GNSS measurements
  • Apply projections and transformations to the GNSS measurements
  • Record a bag-file for further investigation of the results in a separate Notebook Exercise

Contents

Introduction to this workshop

We prepared a rosbag with ... for you to use.

Download the file localization.db3 from here (1.2 GB).

Save this file to your local directory ${REPOSITORY}/bag. This directory will be mounted into the docker container to the path /home/rosuser/bag.

After the download, navigate to the local directory ${REPOSITORY}/docker on your host and execute ./r2_run.sh to start the ACDC docker container.

Inside the container, you can navigate to /home/rosuser/ws/bag and execute ros2 bag info localization.db3 to inspect the rosbag:

Files:             localization.db3
Bag size:          1.2 GiB
Storage id:        sqlite3
Duration:          117.463s
Start:             Sep 26 2023 10:44:22.915 (1695717862.915)
End:               Sep 26 2023 10:46:20.379 (1695717980.379)
Messages:          6051
Topic information: Topic: /camera/image | Type: sensor_msgs/msg/Image | Count: 2375 | Serialization Format: cdr
                   Topic: /gnss/navsatfix | Type: sensor_msgs/msg/NavSatFix | Count: 114 | Serialization Format: cdr
                   Topic: /ground_truth/pose | Type: nav_msgs/msg/Odometry | Count: 2375 | Serialization Format: cdr
                   Topic: /lidar/pointcloud | Type: sensor_msgs/msg/PointCloud2 | Count: 1187 | Serialization Format: cdr

You can see that the rosbag has a duration of 117 seconds and contains 4 different messages:

  • A sensor_msgs/msg/Image on topic /camera/image published with a frequency of 20 Hz. The purpose of the image stream is simply to get a better understanding of the environment the vehicle is moving in.
  • A sensor_msgs/msg/NavSatFix on topic /gnss/navsatfix published with a frequency of 1 Hz. These topic provides information of the vehicle position in a global reference frame.
  • A nav_msgs/msg/Odometry on topic /ground_truth/pose published with a frequency of 20 Hz. This topic is mainly utilized for debugging and evaluation purpose. Keep in mind, that it is really hard to gather ground-truth measurements for pose estimation in real-world. In this case since we captured this bag file using a simulation, we were able to easily derive the actual pose of the vehicle.
  • A sensor_msgs/msg/PointCloud2 on topic /lidar/pointcloud published with a frequency of 10 Hz. THis topic will be the input for LiDAR-Odometry.

In the context of this course, the nav_msgs/msg/Odometry and the sensor_msgs/msg/NavSatFix will be new to you in particular. In the following we will briefly discuss these two message types.

ROS2's nav_msgs/msg/Odometry Message

The message definition nav_msgs/msg/Odometry is part of ROS2's standard messages. It is used for indicating an estimate of a robots (or in our case vehicles) pose and velocities in 3D space. Within this section, this message type is used for the ground-truth pose of the vehicle, as well as an output of the LiDAR-Odometry. Please read the documentation about the detailed message format and it's content.

ROS2's sensor_msgs/msg/NavSatFix Message

Also the message definition sensor_msgs/msg/NavSatFix is part of ROS2's standard messages. It is designed to provide the position derived from any GNSS device. The resulting position is given with respect to the WGS84 system. Feel free to read the documentation about the detailed message format.

ROS2's geometry_msgs/msg/PoseStamped Message

Also the message definition geometry_msgs/msg/PoseStamped is part of ROS2's standard messages. It is designed to provide the pose of a robot (or in our case a vehicle) in a cartestian coordinate system. The pose is composed of a geometry_msgs/msg/Point providing the position and a geometry_msgs/msg/Quaternion providing the orientation. For further information on quaternions please refer to the ROS2 tutorials.

Investigating KISS-ICP LiDAR Odometry

Now that we've investigated the bag-file, and you got an insight in some of the utilized message definitions we're ready to build and run some code.

As already mentioned we will combine GNSS measurements with the output of a LiDAR-Odometry approach to achieve a higher-frequent pose estimate for our vehicle.

Since there are often already many good implementations publicly available, the wheel does not always have to be reinvented! In this case we will use the LiDAR-Odometry pipeline KISS-ICP that is included as submodule (colcon_workspace/section_2/localization/kiss-icp) within our workspace.

Task 1: Configuration of KISS-ICP

We already prepared a launch-file to start the lidar odometry within our workspace: colcon_workspace/section_2/localization/localization/launch/odometry.launch.py.

Starting KISS-ICP LiDAR Odometry is as simple as remapping the input topic of the sensor_msgs/msg/PointCloud2 message. To achieve this just adjust the launch-file in line 41.

# START TASK 1 CODE HERE
remappings=[('pointcloud_topic', '/my_pcl_topic')],
parameters=[{
        'publish_alias_tf': False,
        'publish_odom_tf': True,
        'odom_frame': 'odom',
        'child_frame': 'ego_vehicle/lidar',
        'max_range': 100.0,
        'min_range': 5.0,
        'deskew': False,
        'max_points_per_voxel': 20,
        'initial_threshold': 2.0,
        'min_motion_th': 0.01,
    }],
# END TASK 1 CODE HERE

Moreover you can parameterize the KISS-ICP algorithm with various parameters. Feel free to vary the parameter values later on and observe the influence on the odometry result. For now, you can leave the paremeter values as they are.

When you have successfully edited the launch-file and saved your changes you can navigate to colcon_workspace and build the package with colcon build. Make sure to perform these actions using a terminal window that is attached to your container.

# Run from within the colcon_workspace directory
colcon build --packages-up-to localization --symlink-install

and source the workspace

source install/setup.bash

Now we can launch the LiDAR-Odometry:

ros2 launch localization odometry.launch.py

RViz will open but you won't be able to see anything happen. First we need to play the bag file. There to you can attach a new terminal to the docker container with ./r2_run.sh (if you haven't already).

Again navigate into the colcon_workspace and source the workspace

source install/setup.bash

Navigate into the bag-directory and play the bag-file:

# Run from within the colcon_workspace directory
cd ../bag
ros2 bag play localization.db3 --clock

If everything is configured correctly the output in the RViz window should somehow look like the following:

lidar_odometry_output

The image shows the accumulated pointclouds and the estimated vehicle-trajectory (blue line), that is derived via scan-matching through the ICP algorithm.

Investigating the resulting behavior

Inspect the bag-file until the end. Maybe you will notice something at the second half of the recording? Can you explain the resulting behavior? Maybe a look at the image-stream will help you to explain it!

We'll leave it at that for now since we've successfully launched the KISS-ICP. Feel free to invesitgate the influence of the various parameters on the resulting pose estimate.

Processing of GNSS measurements

In the following tasks we will combine the output of LiDAR odometry with GNSS measurements to obtain a pose estimate that is as accurate and robust as possible.

The following tasks will all modify the source code of the GNSSLocalizationNode.cpp. First we will investigate the processing of incoming GNSS measurements. For each incoming GNSS measurement the gnssCallback is called.

Usually algorithms in an automated driving stack work within cartesian coordinate systems. On the other hand, a GNSS device usually delivers the position-estimate with respect to the WGS84 reference system. For this reason it is necessary to apply a projection from the spherical coordinates to a cartesian coordinate system. In this case we will use the UTM reference system.

In the respective callback the incoming message is initially projected to UTM coordinates by the function projectToUTM. We will implement this function in Task 2.

void GNSSLocalizationNode::gnssCallback(sensor_msgs::msg::NavSatFix::UniquePtr msg)
{
  geometry_msgs::msg::PointStamped utm_point;
  // apply projection to get utm position from the received WGS84 position
  if(!projectToUTM(msg->latitude, msg->longitude, utm_point)) return;
  utm_point.header.stamp=msg->header.stamp;
  geometry_msgs::msg::PointStamped map_point;

As already explained in theory, the UTM reference system divides the world into 60 zones. Furthermore, a distinction is made between northern and southern hemisphere. A position within a UTM zone can then be indicated by two values: northing and easting in meters. It is obvious that depending on where we are in the world these values can take large numerical values. For this reason, local coordinate systems are often used, which are defined relative to the UTM zone.

In this case we're using the carla_map-frame which is shifted but not rotated relative to the coordinate system of the UTM zone. This allows us to work with smaller numerical values in the local vehicle environment. In practice, this local coordinate system can be defined, for example, by the segment of the digital map that is used in the vehicle environment.

The transformation from the UTM into the carla-map-frame is performed through the function transformPoint. We will implement this function in Task 3.

  // transform the utm-position into the carla_map-frame
  // the corresponding transform from utm to carla_map is provided by the tf_broadcaster_node
  if(!transformPoint(utm_point, map_point, "carla_map")) return;
  // publish the gps point as message
  publisher_gnss_point_->publish(map_point);

Up to this point, we have only considered the position of the vehicle. In addition to the position, the knowledge of the orientation of the vehicle is also required. In this example we will only consider the 2D pose. Since GNSS does not provide any information about the orientation of the vehicle per se, we will estimate the vehicle orientation from two sequential GNSS measurements in Task 4. This is performed through the estimateGNSSYawAngle function.

  // Estimate the yaw angle from two gnss-points within the map-frame
  if(last_gnss_map_point_!=nullptr) // We need two gnss-points to estimate the yaw angle --> check if the last_gnss_map_point_ is available
  {
    geometry_msgs::msg::PoseStamped map_pose;
    estimateGNSSYawAngle(map_point, *last_gnss_map_point_, map_pose);
    // store the map_pose in a member variable
    gnss_map_pose_ = std::make_shared<geometry_msgs::msg::PoseStamped>(map_pose);
    publisher_gnss_pose_->publish(*gnss_map_pose_);
    new_gnss_pose_ = true; // flag indicating if a new gnss_pose is available
  }
  // Set the current map_point to the last_gnss_map_point_ for the next iteration
  last_gnss_map_point_ = std::make_shared<geometry_msgs::msg::PointStamped>(map_point);
}

The callback also publishes various topics for visualization, so you can view the intermediate results in RViz.

Now that you have understood the basic processing steps for gnss measurements, you can start with Task 2.

Task 2: Projecting GNSS measurements into the UTM reference frame

As already mentioned, we will implement a function to project GNSS measurements provided with respect to the WGS84 system into the UTM reference system. Again this is a problem for which we can use existing software libraries. In this case we will use the Geographiclib.

Open the GNSSLocalizationNode.cpp at line 101 to implement the desired functionality.

/**
 * @brief Get the UTM Position defined by the given latitude and longitude coordinates
 * The position is transformed into UTM by using GeographicLib::UTMUPS
 * 
 * @param[in] latitude latitude coordinate in decimal degree
 * @param[in] longitude longitude coordinate in decimal degree
 * @param[out] geometry_msgs::msg::PointStamped indicating the position in the utm system
 * @return bool indicating if projection was succesfull
 */
bool GNSSLocalizationNode::projectToUTM(const double& latitude, const double& longitude, geometry_msgs::msg::PointStamped& utm_point)
{
  try {
    // START TASK 2 CODE HERE




    // return true if succesful
    return false;
    // END TASK 2 CODE HERE
  } catch (GeographicLib::GeographicErr& e) {
    RCLCPP_WARN_STREAM(this->get_logger(), "Tranformation from WGS84 to UTM failed: " << e.what());
    return false;
  }
}

Hints:

  • Use the description in the comment above the function header to implement the corresponding functionality.
  • The class GeographicLib::UTMUPS offers the function void GeographicLib::UTMUPS::Forward(double lat, double lon, int& zone, bool& northp, double& x, double& y). The required arguments are given as follows:
    • double lat [input] latitude coordinate
    • double lon [input] longitude coordinate
    • int& zone [output] reference to an integer variable that indicates the corresponding UTM-zone
    • bool& northp [output] reference to a bool variable that indicates if the corresponding UTM-zone is located on the northern hemisphere or not
    • double& x [output] reference to a double variable that gives the resulting easting coordinate in the UTM-zone
    • double& y [output] reference to a double variable that gives the resulting northing coordinate in the UTM-zone
  • The variables zone and northp need to be provided as arguments in GeographicLib::UTMUPS::Forward, but since they are not needed afterwards you can declare the corresponding variables in the local scope of the function. They do not have to be returned!
  • Change the frame_id from the variable utm_point to utm before returning the function, since the position is given relative to the UTM-frame.
    • you can access the frame_id variable by typing utm_point.header.frame_id
  • Make sure to return true if the projection was succesful

Testing:

If you are unsure about your implementation, you can run a simple unit test before proceeding to the next task.

First, make sure that the implemented code compiles without errors. There to run:

# Run from within the colcon_workspace directory
colcon build --packages-up-to localization --symlink-install

If you do not get any compilation errors, open the test_gnss_localization.cpp-file. The implemented test executes the test-function for the WGS84 position latitude = 50.787467 and longitude = 6.046498. In this case we expect a value of 291827.02 for the UTM-x-coordinate and 5630349.72 for the UTM-y-coordinate. Furthermore it is checked if the frame_id is set correctly and the return value of the function is true.

To test your implementation simply paste your implementation into the test-function, rebuild the code,

# Run from within the colcon_workspace directory
colcon build --packages-up-to localization

run the test

# Run from within the colcon_workspace directory
colcon test --packages-up-to localization && colcon test-result --verbose

and inspect the results:

[==========] Running 1 test from 1 test suite.
[----------] Global test environment set-up.
[----------] 1 test from TestGNSSLocalization
[ RUN      ] TestGNSSLocalization.test
/docker-ros/ws/colcon_workspace/src/section_2/localization/localization/test/test_gnss_localization.cpp:38: Failure
Expected equality of these values:
  true
  projectToUTM(latitude, longitude, utm_point)
    Which is: false
/docker-ros/ws/colcon_workspace/src/section_2/localization/localization/test/test_gnss_localization.cpp:39: Failure
The difference between 291827.02 and utm_point.point.x is 291827.02000000002, which exceeds 1e-2, where
291827.02 evaluates to 291827.02000000002,
utm_point.point.x evaluates to 0, and
1e-2 evaluates to 0.01.
/docker-ros/ws/colcon_workspace/src/section_2/localization/localization/test/test_gnss_localization.cpp:40: Failure
The difference between 5630349.72 and utm_point.point.y is 5630349.7199999997, which exceeds 1e-2, where
5630349.72 evaluates to 5630349.7199999997,
utm_point.point.y evaluates to 0, and
1e-2 evaluates to 0.01.
/docker-ros/ws/colcon_workspace/src/section_2/localization/localization/test/test_gnss_localization.cpp:41: Failure
Expected equality of these values:
  "utm"
  utm_point.header.frame_id
    Which is: ""
[  FAILED  ] TestGNSSLocalization.test (0 ms)
[----------] 1 test from TestGNSSLocalization (0 ms total)

[----------] Global test environment tear-down
[==========] 1 test from 1 test suite ran. (0 ms total)
[  PASSED  ] 0 tests.
[  FAILED  ] 1 test, listed below:
[  FAILED  ] TestGNSSLocalization.test

  1 FAILED TEST

Above you will find the output of the test result in case that no implementation has been made. The log of the test says that neither utm_point.point.x and utm_point.point.y are set correctly, because both have the value 0 after the function call. Furthermore the frame_id of the utm_point is not set and the return-value of the function is still set to false.

If implemented correctly, the log of the test function should look like this:

Starting >>> kiss_icp
Finished <<< kiss_icp [0.02s]
Starting >>> localization
Finished <<< localization [0.10s]          

Summary: 2 packages finished [0.27s]
Summary: 2 tests, 0 errors, 0 failures, 0 skipped

Task 3: Transforming an UTM point into a local map frame using tf2

Now that we have transformed the WGS84 position into the UTM coordinate system, we can perform another transformation into the local coordinate system carla_map. This is achieved by implementing the function transformPoint.

Open the GNSSLocalizationNode.cpp at line 126 to implement the desired functionality.

/**
 * @brief This function transforms a given geometry_msgs::msg::PointStamped into a given frame if tf is available
 * 
 * @param[in] input_point 
 * @param[out] output_point 
 * @param[in] output_frame the frame to transform input_point to
 * @return bool indicating if transformation was succesful
 */
bool GNSSLocalizationNode::transformPoint(const geometry_msgs::msg::PointStamped& input_point, geometry_msgs::msg::PointStamped& output_point, const std::string& output_frame)
{
  try {
    // START TASK 3 CODE HERE


    // return true if succesful
    return false;
    // END TASK 3 CODE HERE
  } catch (tf2::TransformException& ex) {
    RCLCPP_WARN_STREAM(this->get_logger(), "Tranformation from '" << input_point.header.frame_id << "' to '" << output_frame << "' is not available!");
    return false;
  }
}

Hints:

  • Use the description in the comment above the function header to implement the corresponding functionality.
  • Use tf2 to perform the transformation. Transformations with tf2 are usually performed in two steps:
    1. retrieve the needed transformation with lookupTransform.
    2. apply the transformation to a corresponding data type with doTransform.
  • lookupTransform is a member of tf2_ros::Buffer. The GNSSLocalizationNode has a member std::unique_ptr<tf2_ros::Buffer> called tf_buffer_. Thus you can call lookupTransform as follows: tf_buffer_->lookupTransform(target_frame, source_frame, time)
    • The target_frame is provided as function argument of transformPoint
    • The source_frame can be derived from the input_point with input_point.header.frame_id
    • Since we would like to lookup the transform matching the time of the measured input_point we pass the input_point.header.stamp as time-argument into lookupTransform
    • lookupTransform returns an object of type geometry_msgs::msg::TransformStamped that should be stored in a variable
  • doTransform is a public function in the tf2-namespace. Thus you can call doTransform as follows: tf2::doTransform(input_point, output_point, transform)
    • the input argument transform is of type geometry_msgs::msg::TransformStamped as returned from lookupTransform
  • The required transform from utm to carla_map is provided by the tf_broadcaster_node

Testing:

This time we will test the correct implementation by running the compiled ROS 2 node. First, make sure that the implemented code compiles without errors. There to run:

# Run from within the colcon_workspace directory
colcon build --packages-up-to localization

After succesful compilation source your workspace

source install/setup.bash

and launch the localization-stack:

ros2 launch localization localization.launch.py

RViz will open but you won't be able to see anything happen. First we need to play the bag file. There to you can attach a new terminal to the docker container with ./r2_run.sh (if you haven't already).

Again navigate into the colcon_workspace and source the workspace

source install/setup.bash

Navigate into the bag-directory and play the bag-file:

# Run from within the colcon_workspace directory
cd ../bag
ros2 bag play localization.db3 --clock

If everything is implemented correctly the output in the RViz window should somehow look like the following:

localization_rviz

The red arrow in the RViz 3D-View visualizes the ground-truth pose of the vehicle, while the purple sphere shows the corresponding transformed gnss position of the vehicle. You will quickly notice that the gnss position is published at a lower frequency compared to the ground-truth position, resulting in the visible position error in the image.

Before we can fuse the lower frequent GNSS measurements with information from odometry in order to obtain a suitable frequency, we first want to estimate a yaw-angle from two sequential gnss positions in Task 4.

Task 4: Estimating the vehicle yaw from sequential GNSS measurements

Within this task we would like to estimate the vehicle yaw-angle based on two sequential GNSS measurments.

In this case the function you will implement requires two geometry_msgs::msg::PointStamped as input arguments and a geometry_msgs::msg::PoseStamped as output argument. In the ROS geometry_msgs definition the orientation of the pose is given as quaternion. First you will calculate the yaw angle based on the two sequential points using trigonometric equations. Afterwards you will have to convert this into a quaternion using tf2.

Open the GNSSLocalizationNode.cpp at line 147 to implement the desired functionality.

/**
 * @brief This function estimates the yaw-angle of the vehicle with respect to two given point-measurements
 * 
 * @param[in] current_point the current GNSS Point
 * @param[in] last_point the previous GNSS Point
 * @param[out] output_pose geometry_msgs::msg::Pose including the current_point and an additional 2D orientation
 */
void GNSSLocalizationNode::estimateGNSSYawAngle(const geometry_msgs::msg::PointStamped& current_point, const geometry_msgs::msg::PointStamped& last_point, geometry_msgs::msg::PoseStamped& output_pose)
{
    // START TASK 4 CODE HERE
    // calculate the yaw angle from two sequential gnss-points




    // use header from input point

    // use the position provided through the input point

    // generate a quaternion using the calculated yaw angle



    // END TASK 4 CODE HERE
}

Hints:

  • You may use the illustration below to derive the trigonometric equations to calculate the yaw-angle
  • Use std::atan2 to calculate the arc tangent of $dy/dx$ since it automatically determines the correct quadrant.
  • Initialize a tf2::Quaternion object
  • The tf2::Quaternion class provides a function setRPY(roll, pitch, yaw) that generates a quaternion from the given roll, pitch and yaw arguments. The function needs to be called on an tf2::Quaternion-Object
  • For roll and pitch use a value of 0
  • Since the the orientation in a geometry_msgs::msg::PoseStamped message is type of geometry_msgs::msg::Quaternion we need to convert the tf2::Quaternion-object into an geometry_msgs::msg::Quaternion. There to the tf2_geometry_msgs-package offers the tf2::toMsg(q) which requires an tf2::Quaternion as function argument and returns the corresponding geometry_msgs::msg::Quaternion.

Combining Odometry and GNSS measurements

Task 5: Predicting the current pose using relative odometry measurements

Hints:

  • ...

Result

...

Wrap-up

  • You learned how ...
  • You learned how ...
  • You learned how to perform ...
  • You have completed a simple ...
  • Feel free to explore the rest of the code for a deeper understanding ...

Automated and Connected Driving Challenges

Clone this wiki locally