ROS wrapper (cpp) for the Fourier Scan Matcher (FSM) in the guise of lidar odometry.

Functionally fsm_lidom_ros assumes a 2D LIDAR sensor with a field of view of 360 degrees and executing it while the sensor is on the move will output an estimate of its trajectory.

Installation

Via Docker

docker pull li9i/fsm_lidom_ros

docker run -it \
    --name=fsm_lidom_ros_container \
    --env="DISPLAY=$DISPLAY" \
    --net=host \
    --rm \
    li9i/fsm_lidom_ros:latest

or build the image with the most recent code of this repository

cd ~/catkin_ws/src
git clone git@github.com:li9i/fsm_lidom_ros.git
cd fsm_lidom_ros/docker
./build_fsm_lidom_image.sh
./run_fsm_lidom_container.sh

Via the traditional method

Tested in Ubuntu 16.04 and ROS kinetic

Dependencies: `CGAL 4.7` `FFTW3` `boost/random`

Building

cd ~/catkin_ws/src
git clone git@github.com:li9i/fsm_lidom_ros.git
cd ~/catkin_ws
catkin build fsm_lidom_ros

Executing

roslaunch fsm_lidom_ros avanti_fsm_lidom.launch

Nodes

`fsm_lidom_node`

Subscribed topics

Topic	Type	Utility
`scan_topic`	`sensor_msgs/LaserScan`	2d panoramic scans are published here
`initial_pose_topic`	`geometry_msgs/PoseWithCovarianceStamped`	optional---for setting the very first pose estimate to something other than the origin

Published topics

Topic	Type	Utility
`pose_estimate_topic`	`geometry_msgs/PoseStamped`	the current pose estimate relative to the global frame is published here
`path_estimate_topic`	`nav_msgs/Path`	the total estimated trajectory relative to the global frame is published here
`lidom_topic`	`nav_msgs/Odometry`	the odometry is published here

Services offered

Service	Type	Utility
`fsm_lidom/clear_estimated_trajectory`	`std_srvs/Empty`	clears the vector of estimated poses
`fsm_lidom/set_initial_pose`	`std_srvs/Empty`	calling this service means: node subscribes to `initial_pose_topic`, obtains the latest pose estimate, sets fsm's initial pose, and unsubscribes
`fsm_lidom/start`	`std_srvs/Empty`	commences node functionality
`fsm_lidom/stop`	`std_srvs/Empty`	halts node functionality (node remains alive)

Parameters

Found in config/params.yaml:

IO Topics	Description
`scan_topic`	2d panoramic scans are published here
`initial_pose_topic`	(optional) the topic where an initial pose estimate may be provided
`pose_estimate_topic`	`fsm_lidom_ros`'s pose estimates are published here
`path_estimate_topic`	`fsm_lidom_ros`'s total trajectory estimate is published here
`lidom_topic`	`fsm_lidom_ros`'s odometry estimate is published here

Frame ids	Description
`global_frame_id`	the global frame id (e.g. `/map`)
`base_frame_id`	the lidar sensor's reference frame id (e.g. `/base_laser_link`)
`lidom_frame_id`	the (lidar) odometry's frame id

FSM-specific parameters	Description
`size_scan`	the size of scans that are matched (execution time is proportional to scan size, hence subsampling may be needed)
`min_magnification_size`	base angular oversampling
`max_magnification_size`	maximum angular oversampling
`num_iterations`	Greater sensor velocity requires higher values
`xy_bound`	Axiswise radius for randomly generating a new initial position estimate in case of recovery
`t_bound`	Angularwise radius for randomly generating a new initial orientation estimate in case of recovery
`max_counter`	Lower values decrease execution time
`max_recoveries`	Ditto

Transforms published

lidom_frame_id <- base_frame_id

in other words fsm_lidom_node publishes the transform from /base_laser_link (or equivalent) to the equivalent of /odom (in this case lidom_frame_id)

Motivation and Under the hood

1 min summary video

IROS 2022 paper

@INPROCEEDINGS{9981228,
  author={Filotheou, Alexandros and Sergiadis, Georgios D. and Dimitriou, Antonis G.}
  booktitle={2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)},
  title={FSM: Correspondenceless scan-matching of panoramic 2D range scans},
  year={2022},
  pages={6968-6975},
  doi={10.1109/IROS47612.2022.9981228}}

kimsoohwan/fsm_lidom_ros