See discussions, stats, and author profiles for this publication at: https://www.researchgate.

net/publication/337225474

Autonomous Navigation Using Monocular ORB SLAM2

Chapter in Lecture Notes in Electrical Engineering · January 2020

DOI: 10.1007/978-981-15-0974-2_6

CITATIONS READS
2 2,678

3 authors, including:

Sneh Soni
Nirma University
9 PUBLICATIONS 14 CITATIONS

SEE PROFILE

All content following this page was uploaded by Sneh Soni on 28 July 2021.

The user has requested enhancement of the downloaded file.

 Autonomous Navigation using Monocular ORB SLAM2

Shubham Vithalani, Sneh Soni, Param Rajpura

Instrumentation & Control Engineering Department, Institute of Technology, Nirma University
[email protected]​, ​[email protected]​, ​[email protected]

Abstract. Simultaneous Localization and Mapping(SLAM) is the mapping of an unknown environment and at the
same time localizing the ego body in that environment. ORB SLAM2 [1] is an state of the art visual SLAM algorithm
which can calculate camera trajectory using Monocular camera. Since monocular slam has the scale drift issue the
source code has been so altered that the map can be saved or previously built map can be reloaded for localization. To
plan optimal trajectory of a vehicle to reach from a source to goal, A* [2] search algorithm has been implemented.
Gazebo [3] is an open-source robot simulation tool on which a Turtlebot robot was used to test algorithm that has been
proposed in this paper. Turtlebot is a two-wheeled differential drive robot on which various sensors are mounted. A
novel approach has been used for local path planning of the vehicle. ROS[4] framework has been used to communicate
between various nodes for performing navigation.

Keywords. ​ORB Slam, ROS [3], Gazebo [3] , A* search [2], SLAM, Mapping, Monocular Camera

1. Introduction
Due to the boom of automation in the industrial sector, autonomous indoor navigation of robots has become one of
the key problem for the service oriented robots. To solve the indoor navigation problem various methods have been
proposed which involve use of odometers [5] which are prone to drift and lack the loop closing property. Other
methods have proposed use of depth sensors [6], lidars [7] which are expensive to incorporate and yet unreliable.
Visual SLAM problem is divided into mapping, localization, path planning, dynamic obstacle avoidance. There are
two methods to solve the SLAM problem: Filtering based method which uses methods like Extended Kalman Filter
[8], Unscented Kalman Filter [9], Particle filter [10] [11] to simultaneously revise the pose and posture of the Key
Feature points in the map such as FAST SLAM proposed by Michael Montemerlo et al [12]. The other method
involves graph optimization using the bundle adjustment method [11] to map the feature point and ego-body
geometry optimization using the error minimization methods, including PTAM (Parallel Tracking & Mapping) [13]
proposes by Georg Klein et al, and ORB-SLAM2 proposed by Raúl Mur-Artal [1].
Robot Operating system(ROS) is the universal platform for robotic applications as it is an open-source framework. It
is a set of utilities and libraries for implementing all different kinds of functionality on robots. Its influence has been
growing rapidly in the robotics community. It is being used in this work for communicating between ORB SLAM
node, Global Planning Node, Navigation Node.
Gazebo is a robot simulation tool on which Turtlebot which is a differential drive two-wheeled robot was simulated.
On the robot body a camera is mounted to capture images and simultaneously transfers it to ORB SLAM2 Node.
Although various methods of SLAM exists which uses depth sensor such as Kinect [15] or Intel Realsense[16] that
costs around 200-250 USD. Proposed algorithm does not necessarily require a depth sensor and high level of
accuracy of navigation was achieved despite using the Monocular camera that costs around 20 USD.
First in the mapping stage, environment using ORB-Slam2 and the trajectory so traversed is saved. The associated
mappoints and keyframes of the environment are saved. In the navigation stage the saved map is loaded, hence the
robot localises itself in the map. The algorithm adapts itself to changing objects in the environment every time it
performs navigation because the last traversed keyframes and mappoints are updated.
The algorithm was tested on a laptop having Intel i5 processor with 4GB RAM and worked satisfactorily well. Thus
the system eliminates the use of high-end GPU.
2. ORB SLAM
ORB-SLAM, is a feature-based SLAM system that runs in indoor and outdoor environments. The system is highly
robust which can run in real time. The system is so developed that it uses the same features for performing all
SLAM tasks like tracking, relocalisation and loop closing which makes the system simple, reliable and accurate.
The system uses ORB [17] (Oriented and Rotated Brief) features which allows real-time performance without the
need of GPUs. It has basically two parts:
Keyframes: Keyframes store the information about the camera poses, extracted ORB features which minimize
informational redundancy.
Mappoints: A map points represents a 3D point in world coordinates and stores the information of all the keyframes
from which it was observed.
Keyframes and map points together are used to form co-visibility graph. Each keyframe acts as a node and nodes are
connected if there are at least 15 map point observations. A spanning tree is created that starting with the initial
keyframe connects only those keyframes which share the most map point observations [1] [18].
ORB SLAM uses three parallel threads which are: Tracking, Local Mapping, Loop Closing.

Fig. 1 ORB SLAM System Flow

The ORB Slam 2 system developed by Raúl Mur-Artal is shared as an open-source project on Github. The system
does not support map saving and reloading previously saved maps. Hence the source code was edited in order to
meet the same using boost serialization. Thus the map save/load feature was enabled.
The current pose of robot was published on a ROS topic. So as Global Planning node and Navigation node can know
the current position of the robot.
The path traversed by the robot in the mapping stage is saved. The global planners give the optimal path coordinates
through which robot must traverse.
 Fig. 4 Green points are map points Fig. 5 Blue pyramids are Keyframes and Red are Mappoints

3. Gazebo Simulation and Turtlebot

Gazebo [3] is an open-source 3D robotics simulation tool and uses multiple high-performance physics engines such
as ODE(Open Dynamics Environment) [19], Bullet. It provides real-life rendering of environments. It can even
model sensors the way it perceives the environment such as lasers, cameras, depth sensors. Since ORB SLAM relies
heavily on feature points, an environment was created with models having lots of features. It can also simulate
various lighting conditions and various textured models.
Turtlebot is an two-wheeled differential drive robot which can be simulated on Gazebo. The robot has a in-built
camera sensor, 3D sensor, laser sensor the data of which is published as various ROS topics. The robot takes linear
velocity, angular velocity as input commands for driving the mobile base.

Fig. 2 Turtlebot robot in a simulated environment Fig. 3 Isometric view: Turtlebot Robot

4. Mapping, Navigation Stage

In the case of Monocular SLAM, ORB-SLAM2 defines the scale arbitrarily as the median scene depth. Thus
traversing the same map again the mappoints and keyframes data need to be saved so as to avoid scale drift. Gazebo
publishes the images obtained from the camera to a ROS Topic. The ORB SLAM Node processes these stream of
images and forms mappoints and keyframes. The mappoints and keyframes data is saved to a binary file so that they
can be reloaded. The Keyframe trajectory data consists of Timestamp(Time), position coordinates(x, y, z), rotation
quaternions(qx, qy, qz, qw).
 Fig. 6 Mapping Flowchart
The navigation stage involves three ROS nodes: ORB SLAM Node, Global Path Planning, Navigation Node.

Fig.7 Robot Navigation Structure

5. Global Path Planner
A* [2] search is a smart search algorithm which is used for finding the optimal path for traversing between a source
to destination. It adds up the data which Dijkstra’s Algorithm [20] uses (points which are close to the source are
selected) and the data which Greedy Best-First search uses (points which are close to the goal are selected).
g(n) is the cost of the path from source point to any vertex n
h(n) is the heuristic estimated cost from vertex n to the goal.
A* balances the two as it moves from starting point to goal and selects the node with minimum, f(n)=g(n) + h(n)
Global Planner acts as a ROS node, it acquires current pose data from the ORB SLAM2 Node. Goal coordinates are
given by a user using a simple GUI. The global planners plans the optimal route from source to destination and
returns the Keyframe indexes that the Navigation node needs to follow in order to reach the destination.
The following figures depicts the trajectory generated by A* search algorithm for various source-destination
combination.
 Fig. 8 Trajectories produced by Global Planner. Green cross is the source and Blue is destination
6. Navigation Planner
Navigation planner node acquires the current pose data from ORB SLAM2 Node and path poses from Global
Planner node. From the current pose it calculates the nearest point in the path poses data using euclidean distance
method. From the nearest point, 3rd next point in the path poses data is targeted as the local goal. The local goal is
then passed as goal to ROS Action Server.

Use of action server has become default for

algorithm involving navigation. The navigation node
acts as a Action Client. The Action server uses the
following algorithm to move towards the local goal.
Navigation node acts as a server to ROS Action
Client. The advantage of ROS Action server is that
server returns the status, feedback, result for the
given goal.Also using ROS server, a goal can be
cancelled using client request.

Angle to goal = atan2 (gy-sy, gx-sx)

Fig. 9 Local Path Navigation

The action server publishes the velocity commands that the vehicle must undertake in order to reach the next local
goal. The local goal keeps on changing as the current pose of the robot changes. Thus the local goal is always in the
path and ahead of current pose which keeps the robot on the desired path. The action server is ended when the robot
finally reaches the destination.

7. Results
The path traced by vehicle is benchmarked using odometry data present of the vehicle. The absolute positional error
of the vehicle was found to be less than 1 percent of the total path length. The results of the above experiments were
as follows:
Sr. Total path length Total path length Path Traced Path Traced Absolute error Relative Error
No. (SLAM) (in cm) (SLAM) (in cm) (in cm) (in percentage)

1 1.272 630.64 1.251 628.67 1.97 0.3

 2 0.691 343.37 0.708 345.08 1.71 0.49

3 0.987 489.34 0.982 488.96 0.38 0.07

4 0.214 105.93 0.217 106.4 0.47 0.443

5 1.141 698.4 1.148 700.56 2.16 0.31

Fig. 10 Trajectory 1 traversed by vehicle in the map

Fig. 11 Trajectory 2 traversed by vehicle in the map

8. Conclusion
We present a novel approach using which navigation can be performed using Monocular ORB SLAM2. ORB
SLAM2 is an state of the art algorithm for ego-motion detection. We harnessed its potential for navigation of a
View publication stats

robot. The positional error of the robot was found to be within +/- 3 cm in various test drives. The algorithm
successfully searched for optimal path using a map and the robot successfully followed that path. Despite being
monocular camera, the algorithm performed fairly with respect to the accuracy of navigation. The algorithm uses the
map of the last mapping stage thus it adapts to changing objects. It was also tested in varying lighting conditions
which can be simulated on Gazebo and the accuracy of navigation was still within threshold range of error.

9. References

1. Mur-Artal, Raul and Juan D. Tardós. “ORB-SLAM2: An Open-Source SLAM System for Monocular,
Stereo, and RGB-D Cameras.” ​IEEE Transactions on Robotics​ 33 (2017): 1255-1262.
2. Goldberg, Andrew V. and Chris Harrelson. “Computing the shortest path: A search meets graph theory.”
SODA​ (2005).
3. Koenig, Nathan P. and Andrew G. Howard. “Design and use paradigms for Gazebo, an open-source
multi-robot simulator.” 2004 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)
(IEEE Cat. No.04CH37566) 3 (2004): 2149-2154 vol.3.
4. Quigley, Morgan et al. “ROS: an open-source Robot Operating System.” (2009)
5. Masunga, Nsingi. “Mobile Robot Navigation in Indoor Environments by using the Odometer and
Ultrasonic data.” (1999).
6. Biswas, Joydeep and Manuela M. Veloso. “Depth camera based indoor mobile robot localization and
navigation.” ​2012 IEEE International Conference on Robotics and Automation​ (2012): 1697-1702.
7. Gatesichapakorn, Sukkpranhachai et al. “ROS based Autonomous Mobile Robot Navigation using 2D
LiDAR and RGB-D Camera.” ​2019 First International Symposium on Instrumentation, Control, Artificial
Intelligence, and Robotics (ICA-SYMP)​ (2019): 151-154.
8. Weingarten, Jan W. and Roland Siegwart. “EKF-based 3D SLAM for structured environment
reconstruction.” 2005 IEEE/RSJ International Conference on Intelligent Robots and Systems (2005):
3834-3839.
9. Cheng, Jiantong et al. “Compressed Unscented Kalman filter-based SLAM.” 2014 IEEE International
Conference on Robotics and Biomimetics (ROBIO 2014) (2014): 1602-1607.
10. Törnqvist, David et al. “Particle Filter SLAM with High Dimensional Vehicle Model.” Journal of
Intelligent and Robotic Systems 55 (2009): 249-266.
11. Strasdat, Hauke et al. “Visual SLAM: Why filter?” ​Image Vision Comput.​ 30 (2012): 65-77.
12. Montemerlo, Michael et al. “FastSLAM: A Factored Solution to the Simultaneous Localization and
Mapping Problem.” ​AAAI/IAAI​ (2002).
13. Triggs, Bill et al. “Bundle Adjustment - A Modern Synthesis.” Workshop on Vision Algorithms (1999).
14. Klein, Georg and David William Murray. “Parallel Tracking and Mapping for Small AR Workspaces.”
2007 6th IEEE and ACM International Symposium on Mixed and Augmented Reality​ (2007): 225-234.
15. Izadi, Shahram et al. “KinectFusion: real-time 3D reconstruction and interaction using a moving depth
camera.” ​UIST​ (2011).
16. Murray, Don Ray and James J. Little. “Using Real-Time Stereo Vision for Mobile Robot Navigation.”
Auton. Robots​ 8 (2000): 161-171.
17. Rublee, Ethan et al. “ORB: An efficient alternative to SIFT or SURF.” 2011 International Conference on
Computer Vision (2011): 2564-2571.
18. Wrobel, Bernhard P.. “Multiple View Geometry in Computer Vision.” KI 15 (2001): 41.
19. Smith, Russell L.. “Open Dynamics Engine-ODE.” (2005).
20. Sniedovich, Moshe. “Dijkstra's algorithm revisited: the dynamic programming connexion.” (2006).