icp based robot localization
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).
There are many stellar packages for localization which rely on icp.
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.
A typical icp algorithm consists of two steps:
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.
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.
map/topic
string
map
scan/topic
string
scan
odom/map_frame
string
map
odom/base_frame
string
base_link
odom/base_frame
string
odom
map/k
int
map/radius
int
icp/t_norm
icp/t_norm
and icp/r_norm
the
icp is said to be converged.float
icp/r_norm
icp/t_norm
])float
icp/max_iter
icp/t_norm
and icp/r_norm
int
icp/stride
int
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-mapscan
: 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.