dorezyuk / i_see_pee

icp based robot localization

i_see_pee

i_see_pee is a 2d localization package. At its core it uses an icp algorithm to determine the current robot position. Its main advantage is that it performs one optimization step linear in time: O(N).

Whats the difference

There are many stellar packages for localization which rely on icp.

• libpointmatcher
• cartographer
• dynamic_robot_localization

These are generic localization/slam packages. They support 2d and 3d environment, offer rich features for sensor processing and what not. Which is nice. i_see_pee is different. It just offers 2d localization. But its smaller. And its way faster.

The speed

A typical icp algorithm consists of two steps:

• k nearest neighbors matching
• solving the least squares problem.

The least squares problem is typically O(N), which is nice. The k nearest neighbors is a little more expensive. Typically the lookup is implemented using a knn-tree. The lookup there comes at O(D log M), where D is the dimension and M the size of the occupied points on the map.

When aiming for a generic setup, there is little one can do. i_see_pee leverages the fact that all grid_maps are discrete and that the registration cloud (map) is known in advance. It pre-computes for cells close to walls the k nearest neighbors and stores them in a hash-table. When a sensor reading falls in one of these cells, all its k nearest neighbors can be retrieved in linear time.

Compiling

You will require Eigen for the compilation. Besides that the package can be compiled as a standard ros-package.

If you use catkin tools you can compile the package with:

cd catkin_ws/src
git clone https://github.com/dorezyuk/i_see_pee.git
catkin build i_see_pee -DCMAKE_BUILD_TYPE=Release

Hint: make sure to compile the library as release

Depending on your processor you should enable vectorization for Eigen.

Configuring

Subscriptions

map/topic

• topic under which the static map is published
• type: string
• default: map
• message_type: nav_msgs::OccupancyGrid

scan/topic

• topic under which the sensor data is published
• type: string
• default: scan
• message_type: sensor_msgs::LaserScan

Frames

odom/map_frame

• frame_id which defines the global frame.
• type: string
• default: map

odom/base_frame

• frame_id which defines the base frame. You can use either only odometry, which is provided by the differential_drive controller or fuse imu and odometry to improve the initial guess.
• type: string
• default: base_link

odom/base_frame

• frame_id of the odom frame of the robot.
• type: string
• default: odom

Mapping parameters

map/k

• number of neighbors to consider
• type: int
• default: 5 (min: 1, max: 10)

map/radius

• number of cells to consider from an occupied cell. This massively determines the size of the hash map
• type: int
• default: 5 (min: 0, max: 10)

Optimization parameters

icp/t_norm

• translation norm in meter defining convergence of the icp: if the generated transform is smaller than icp/t_norm and icp/r_norm the icp is said to be converged.
• type: float
• default: 0.01 (min: 0.01)

icp/r_norm

• rotation norm defining convergence (see [icp/t_norm])
• type: float
• default: 0.01 (min: 0.01)

icp/max_iter

• defines the maximum number of iterations of the icp optimization. The looping stops, if the norm of the generated correction transform is smaller then icp/t_norm and icp/r_norm
• type: int
• default: 100 (min: 1, max: 100)

icp/stride

• in order to speed up the computation one can reduce the point cloud size. Applying a stride of 'n' will take only every nth point into consideration. The starting point defines hence which points will be drawn. It is drawn at random.
• type: int
• default: 3 (min: 1, max: 100)

Understanding

When you want to play with the code, you should be lucky: i_see_pee is very small (less then 1000 loc currently). As you might find, everything related to specific ros-types (msgs, tf) is shielded away behind an interface.

The main files are undoubtedly i_see_pee.hpp/cpp. They contain several namespaces:

• map: everything related to the static map. This includes a ros-interface and the algorithm to extract knns from a grid and to store them in a hash-map
• scan: everything related to the sensor data. This includes the ros-interface to sensor_msgs::LaserScan and some caching to convert it to some internal representation.
• odom: everything related to frames/odometry.
• icp: everything related to the math part (which was heavily borrowed from libpointmatcher)

Let me know how your experience was with the package.

i_see_pee's Issues

It makes matching localization perfectly but conflict with amcl

when we use i_see_pee with amcl the error occurs. In Rviz even though the lidar scan is perfectly matched, the robot is teleporting back and forward nonstop. One location is correct the other is amcls location probably. Do you know how we can bypass it?