• 设为首页
  • 点击收藏
  • 手机版
    手机扫一扫访问
    迪恩网络手机版
  • 关注官方公众号
    微信扫一扫关注
    迪恩网络公众号

dorezyuk/i_see_pee: icp based robot localization

原作者: [db:作者] 来自: 网络 收藏 邀请

开源软件名称(OpenSource Name):

dorezyuk/i_see_pee

开源软件地址(OpenSource Url):

https://github.com/dorezyuk/i_see_pee

开源编程语言(OpenSource Language):

C++ 97.7%

开源软件介绍(OpenSource Introduction):

i_see_pee Build Status

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.

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

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.




鲜花

握手

雷人

路过

鸡蛋
该文章已有0人参与评论

请发表评论

全部评论

专题导读
热门推荐
阅读排行榜

扫描微信二维码

查看手机版网站

随时了解更新最新资讯

139-2527-9053

在线客服(服务时间 9:00~18:00)

在线QQ客服
地址:深圳市南山区西丽大学城创智工业园
电邮:jeky_zhao#qq.com
移动电话:139-2527-9053

Powered by 互联科技 X3.4© 2001-2213 极客世界.|Sitemap