This package contains a ROS wrapper for ORB-SLAM2.
ORB-SLAM2 provides a VSLAM (Visual Simultaneous Localization and Mapping) as a ROS node. Using ORB-SLAM2, you can create a 3-D feature-based sparse map, 2-D occupancy grid map (like a building floorplan) from camera data collected by a mobile robot.
Original package already added some features to the ORB-SLAM2. It's authors are: Jan Brehmer, Christopher-Eyk Hrabia, Sebastiano Barrera. I modified it in matters of my MSc thesis.
Binary version of vocabulary file was proposed by poine.
You can find a wide description of mechanisms used in ORB-SLAM2 in the following paper:
Raúl Mur-Artal, J. M. M. Montiel and Juan D. Tardós. ORB-SLAM: A Versatile and Accurate Monocular SLAM System. IEEE Transactions on Robotics, vol. 31, no. 5, pp. 1147-1163, 2015
The package above was tested on Ubuntu 16.04 with ROS Kinetic only with monocular cameras.
To use this package, you need a camera to fill a ROS topic with image data. The package's node will attempt to reconstruct environment's geometry as a point cloud.
Due to the fact that I didn't use the Gazebo simulation software but a real data to test the algorithm performance this is one of the image sequences I recorded and widely used during development process: download from mega.nz. The bag file represents data produced by the robot agent and contains such topics:
/raspicam_node/camera_info : sensor_msgs/CameraInfo
/raspicam_node/image/compressed : sensor_msgs/CompressedImage
/raspicam_node/image/raw : sensor_msgs/Image
/raspicam_node/parameter_descriptions : dynamic_reconfigure/ConfigDescription
/raspicam_node/parameter_updates : dynamic_reconfigure/Config
/robot/range/ir_left : sensor_msgs/Range
/robot/range/ir_right : sensor_msgs/Range
/robot/range/sonar_front : sensor_msgs/Range
/robot/range/sonar_left : sensor_msgs/Range
/robot/range/sonar_right : sensor_msgs/Range
/robot/wheel_left/encoder : std_msgs/Float64
/robot/wheel_left/velocity : std_msgs/Float32
/robot/wheel_right/encoder : std_msgs/Float64
/robot/wheel_right/velocity : std_msgs/Float32`
For evaluation purposes I suggest running the bag along with the ORB-SLAM2 and differential drive robot controller from my another repository (diff_drive_mobile_robot). The full system will be started after running these launches (it's an example only):
roslaunch diff_drive_mapping_robot robot_offline.launch
rosbag play ${PATH TO THE BAG FILE}
roslaunch orb_slam2_ros raspicam_mono_wide.launch
The installation process is quite a mess now. Although, try to follow the instructions below and everything should be good. Note that the installation script was tested under Ubuntu 16.04 with ROS Kinetic and works fine. I also have 3.4.5 version of openCV installed (the installation script is placed in Wiki, but may needs modifications - i.e. not tested lately).
Firstly, clean the existing catkin workspace if you plan to install ORB-SLAM2_ROS along with other packages.
catkin clean
The installation process, in steps, is as follows (in case of an existing workspace, move on to the next block):
mkdir ${YOUR_**NEW**_CATKIN_WORKSPACE_LOCATION}
cd ${YOUR_**NEW**_CATKIN_WORKSPACE_LOCATION}
catkin init
mkdir src
Being in the main folder of your catkin workspace:
cd src
git clone https://github.com/rayvburn/ORB-SLAM2_ROS
Next, clone the octomap_ros repository into the workspace (if you haven't installed it via apt previously):
git clone --single-branch --branch $ROS_DISTRO-devel git@github.com:OctoMap/octomap_ros.git
Create execution privileges for all installation scripts:
cd ORB-SLAM2_ROS/ORB_SLAM2
sudo chmod +x build*
Then run the main build script:
./build_catkin.sh
After successful installation, run an example:
roslaunch orb_slam2_ros ${YOUR_CAMERA_SPECIFIC_LAUNCH_FILE}
The orb_slam2_ros node takes in sensor_msgs/Image messages and builds:
- a point cloud (sensor_msgs/PointCloud2),
- an octomap (octomap_msgs/Octomap),
- a map (nav_msgs/OccupancyGrid). The map can be retrieved via a ROS topic.
image_topic (sensor_msgs/Image) - name of the topic is set as the topic/image_topic parameter
(OPTIONAL) odometry_topic (nav_msgs/Odometry) - name of the topic is set as the map_scale/odom_topic parameter; odometry data is used only when a scale correction for a monocular camera is performed
cam_path (nav_msgs/Path) - stores the camera path since initialization (or scale correction finish)
clear_cam_path (std_msgs/Bool) - publishing true to this topic clears the camera path buffer
frame (sensor_msgs/Image) - the current frame with keypoints marked
info/frame_keypoints (std_msgs/UInt32) - a number of keypoints in the current frame
info/loop_closed (std_msgs/Bool) - publishes true if GBA is running
info/map_keyframes (std_msgs/UInt32) - number of total keyframes in the map
info/matched_points (std_msgs/UInt32) - number of matched map points in the current frame
info/state (orb_slam2_ros/ORBState) - state of the node
info/state_description (std_msgs/String) - text description of the state
map (sensor_msgs/PointCloud2) - the point cloud storing all the map points
map_updates (sensor_msgs/PointCloud2) - the point cloud storing reference map points
octomap (octomap_msgs/Octomap) - the octomap created from the point cloud
projected_map (nav_msgs/OccupancyGrid) - a floorplan created from the octomap as an intersection between 2 heights
projected_morpho_map (nav_msgs/OccupancyGrid) - the projected map with morphological operations performed on
switch_mode (std_msgs/Bool) - publishing true switches to localization mode, false switches back to SLAM mode; SLAM is default value
ORB-SLAM2/orb_slam2_ros/settings/orb_slam2_param.yaml
~topic/freq (float, default: ) - frequency of SLAM system job
~topic/image_topic (string, default: ) - image topic name
~topic/orb_state_republish_rate (float, default: ) - re-publish state @ Hz
~map_scale/perform_correction (bool, default: true) - possible to do with wheel encoders and robot description
~map_scale/odom_topic (string, default: "/odom") - topic that odometry data are published on (valid if correction set true)
~map_scale/scaling_distance (float, default: 1.0) - distance to move according to odom topic, to perform scale estimation
~map_scale/set_manually (float, default: 1.5) - manually set map scale - how visually sparse the map is; no effect if
~map_scale/camera_height (float, default: 0.205) - camera height above base_link frame (it is automatically checked in tf_tree if scale correction is performed)
~map_scale/camera_height_multiplier (float, default: 1.0) - just for better visualization (if the PCL is too low) - scale is not always perfect
~frame/map_frame (string, default: "/orb_slam2/map") - global static map
~frame/map_frame_adjusted (string, default: "/orb_slam2/odom") - name of the interface frame between /map and /odom
~frame/base_frame (string, default: "/orb_slam2/base_link") - robot base frame
~frame/camera_frame (string, default: "/orb_slam2/camera") - optical (see REP103) frame of camera
~octomap/enabled (bool, default: true) - if set false - octomap, projected map and gradient map will not be published
~octomap/publish_octomap (bool, default: false) - octree's voxels visualization
~octomap/publish_projected_map (bool, default: true) - map created as an aggregating cut through a z-interval; configurable additional morphological operations
~octomap/publish_gradient_map (bool, default: false) - map created as height gradients of PCL points
~octomap/rebuild (bool, default: false) - clear octomap when tracking is lost and rebuild
~octomap/rate (float, default: 1.0) - rate of octomap cycles (integrate MapPoints and publish)
~octomap/resolution (float, default: 0.1) - side of a square in meters; how many meters in real world represent px of map
~occupancy/projected_map/min_height (double, default: -10.0) - an aggregating cut through a z-interval
~occupancy/projected_map/max_height (double, default: +10.0) - an aggregating cut through a z-interval
~occupancy/projected_map/morpho_oprations/erode_se_size (int, default: 3) - size of the structuring element, default shape is RECTANGLE
~occupancy/projected_map/morpho_oprations/erode_nb (int, default: 1) - how many 'erode' operations to perform
~occupancy/projected_map/morpho_oprations/open_se_size (int, default: 3) - size of the structuring element, default shape is RECTANGLE
~occupancy/projected_map/morpho_oprations/open_nb (int, default: 1) - how many 'open' operations to perform
~occupancy/projected_map/morpho_oprations/close_se_size (int, default: 3) - size of the structuring element, default shape is CROSS
~occupancy/projected_map/morpho_oprations/close_nb (int, default: 1) - how many 'close' operations to perform
~occupancy/projected_map/morpho_oprations/erode2_se_size (int, default: 3) - size of the structuring element, default shape is ELLIPSE
~occupancy/projected_map/morpho_oprations/erode2_nb (int, default: 1) - how many 'erode' operations to perform
~occupancy/height_gradient_map/max_height (float, default: 0.0) - maximal voxel-z to consider in gradient-based projection
~occupancy/height_gradient_map/nb_erosions (int, default: 1) - number of erosions performed before computing gradients
~occupancy/height_gradient_map/low_slope (float, default: M_PI / 4.0) - lower bound for a slope being considered obstacle-ish
~occupancy/height_gradient_map/high_slope (float, default: M_PI / 3.0) - lower bound for a slope being considered a full solid obstacle
interface_frame - the tf from odometry frame to camera_optical frame (check REP103 for a camera_optical frame orientation - http://www.ros.org/reps/rep-0103.html, example is in my package diff_drive_mapping_robot/tf_broadcaster)
map -> interface_frame (map_odom_interface_frame)
The package need exhaustive testing with other camera types. It also doesn't have the save/load map feature. Please notice that octomap parameters have huge impact on performance (its resolution) and morphological operations (erosion, opening, closing) have big influence how the occupancy grid map looks like. Another thing is that height of the camera often drift after some time and it is hard to set the occupancy/projected_map heights to perfectly map the real environment.
What's novel in this package is the map's scale correction procedure (for monocular cameras) that uses simple odometry sensors to calculate the multiplier between real and map distance.