ORB-Slam2: Implementation on my Ubuntu 16.04 with ROS Kinect
Original code Source: https://github.com/raulmur/ORB_SLAM2
My modified code: https://github.com/jzijlmans/orb_slam2_mod
Installing
The ORB-Slam can be installed by just following the installation process on the github site (see source). You do have to carefully watch the installation of the dependent packages. These can appear to install smoothly, but error messages hidden in all the output can show otherwise. Especially take care with the version of OpenCV that you have installed. It is possible to install newer versions of OpenCV next to older versions (so having two versions installed at the same time), this breaks the Slam installation, thus I have solved this by uninstalling both versions and re-installing the newest version. Also take care in making sure that Pangolin is installed correctly (through https://github.com/stevenlovegrove/Pangolin ) and every error-message is dealt with. (solutions found by googling for the correct github/issue).
Using the ORB-Slam2 monocular on the examples
There are multiple ways to use the slam algorithm. It is a library, so you can write your own program that uses the algorithm (see further for how-to), it can also be executed as a ROS node.
There are multiple example executables delivered with the code (in the /orb_slam2/Examples
folder) which show how to use the algorithm in a program. First of all I tried it on the freiburg1 xyz dataset from the TU-Munich, downloaded from: https://vision.in.tum.de/data/datasets/rgbd-dataset/download . This is one of the examples so the needed configuration files are already provided. The slam algorithm is executed with the following command:
./Examples/Monocular/mono_tum Vocabulary/ORBvoc.txt Examples/Monocular/TUMX.yaml PATH_TO_SEQUENCE_FOLDER
The ./Examples/Monocular/mono_tum
is the path to the executable, you have to be in the orb_slam2 main folder to use exactly this command. (otherwise enter the (relative) path to the orb_slam2 folder after the point.)
The Vocabulary/ORBvoc.txt
points to the `vocabulary' file. What this file exactly does is unclear for me (kind of setup-file, full of numbers??)
The Examples/Monocular/TUMX.yaml
points to a.yaml file containing the settings such as the camera parameters (as for example from OpenCV) and the orb-slam2 settings. The X needs to be replaced with a 1 for the xyz example-dataset. An example of this file:
%YAML:1.0#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------# Camera calibration and distortion parameters (OpenCV)
Camera.fx: 517.306408
Camera.fy: 516.469215
Camera.cx: 318.643040
Camera.cy: 255.313989Camera.k1: 0.262383
Camera.k2: -0.953104
Camera.p1: -0.005358
Camera.p2: 0.002628
Camera.k3: 1.163314# Camera frames per second
Camera.fps: 30.0# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
Camera.RGB: 1#--------------------------------------------------------------------------------------------
# ORB Parameters
#--------------------------------------------------------------------------------------------# ORB Extractor: Number of features per image
ORBextractor.nFeatures: 1000# ORB Extractor: Scale factor between levels in the scale pyramid
ORBextractor.scaleFactor: 1.2# ORB Extractor: Number of levels in the scale pyramid
ORBextractor.nLevels: 8# ORB Extractor: Fast threshold
# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response.
# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST
# You can lower these values if your images have low contrast
ORBextractor.iniThFAST: 20
ORBextractor.minThFAST: 7#--------------------------------------------------------------------------------------------
# Viewer Parameters
#--------------------------------------------------------------------------------------------
Viewer.KeyFrameSize: 0.05
Viewer.KeyFrameLineWidth: 1
Viewer.GraphLineWidth: 0.9
Viewer.PointSize:2
Viewer.CameraSize: 0.08
Viewer.CameraLineWidth: 3
Viewer.ViewpointX: 0
Viewer.ViewpointY: -0.7
Viewer.ViewpointZ: -1.8
Viewer.ViewpointF: 500
Finally there is the PATH_TO_SEQUENCE_FOLDER
part which is where you would have to place the path to the folder containing a tekst file called rgb.txt
and a folder called rgb
. The file should list all the images that needs to be used in the right sequence which all are located in the folder. As an example of the tekst file, the first couple of lines of the tekstfile from the xyz dataset are:
# color images
# file: 'rgbd_dataset_freiburg1_xyz.bag'
# timestamp filename
1305031102.175304 rgb/1305031102.175304.png
1305031102.211214 rgb/1305031102.211214.png
1305031102.243211 rgb/1305031102.243211.png
1305031102.275326 rgb/1305031102.275326.png
1305031102.311267 rgb/1305031102.311267.png
1305031102.343233 rgb/1305031102.343233.png
1305031102.375329 rgb/1305031102.375329.png
1305031102.411258 rgb/1305031102.411258.png
1305031102.443271 rgb/1305031102.443271.png
1305031102.475318 rgb/1305031102.475318.png
1305031102.511219 rgb/1305031102.511219.png
1305031102.543220 rgb/1305031102.543220.png
1305031102.575286 rgb/1305031102.575286.png
1305031102.611233 rgb/1305031102.611233.png
1305031102.643265 rgb/1305031102.643265.png
1305031102.675285 rgb/1305031102.675285.png
1305031102.711263 rgb/1305031102.711263.png
When this is ran for the xyz dataset, the following point cloud it created. It is noteworthy that the camera movement is tracked correctly and multiple scenery objects are recognizable (such as the keyboard or screens). However it does need multiple initialization attempts at the start.
Using ORB-Slam2 monocular on Samsung S5 video
Creating the image folder and rgb.txt
The python script I wrote for the LSD-Slam that converts videos to images is adjusted to also create the rgb.txt
file (with the timestamps and corresponding frames) and place the images inside a folder named rgb
. The file can be ran by using:
python Videotofiles.py <path_to_video> <path_to_output_folder> orb-slam2
The content of Videotofiles.py:
#Create a folder with image files from the video.
#Based on jasper jia code on https://stackoverflow.com/questions/31432870/ros-convert-video-file-to-bag
#Usage: Videotofile.py videopath imagefolder#imports:
import time, sys, os
from ros import rosbag
import roslib, rospy
roslib.load_manifest('sensor_msgs')
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2def CreateVideoimages(videopath, imagefolder,orb):print "capuring video"
cap = cv2.VideoCapture(videopath) #capture the videofile
cb = CvBridge()
# for finding cv version
#(major_ver, minor_ver, subminor_ver) = (cv2.__version__).split('.')
#print major_verfps = cap.get(cv2.CAP_PROP_FPS) #get the fps of the videofile
if fps != fps or fps <=1e-2:
print "Warning: can't get fps"
else:
print "fps achieved succesfull: fps = "+ str(fps)
#start a Window to show the prossesed images in
cv2.startWindowThread()
cv2.namedWindow('img', cv2.WINDOW_NORMAL)#create the imagefolder if it doesnt exists
if not os.path.exists(imagefolder):
os.makedirs(imagefolder)#If nessesary, open the rgb.txt file for Writing
if orb:
print "Opening textfile"
rgbtxtloc = imagefolder[:-3] + "/rgb.txt"
textfile = open(rgbtxtloc,'w')print "Starting creating imgs"
newframeavailable = True #boolean if there are new frames available
frame_id = 0
#loop through all the frames in the videofile
while(newframeavailable): #as long as there are new frames availabe
timestamp = cap.get(cv2.CAP_PROP_POS_MSEC) #get the current timestamp
newframeavailable, frame = cap.read() #get new frame if availabe and update bool
if not newframeavailable: #If there are now new frames
break #stop the while loop
#There are new frames
frame_id += 1 #create a new frame id
imagename = imagefolder + "/frame{0}.jpg".format(frame_id)
written = cv2.imwrite(imagename , frame)
if not written:
print "Writing frame number " + str(frame_id) + " failed"if orb: #write new line in rgb.txt
textfile.write(str(timestamp/1000) + " rgb/frame{0}.jpg\n".format(frame_id))cv2.imshow('img',frame) #show the prossesed frame
cv2.waitKey(1)
#All frames have been prossesed
print "Done creating images, closing up..."
cv2.destroyAllWindows() #close the window
cap.release() #close the capture video
if orb: #close the text file
textfile.close()if len(sys.argv) == 3: #if the user has provided enough arguments
#extract the arguments
videopath = sys.argv[1]
imagefolder = sys.argv[2]
#run the CreateVideoBag function
CreateVideoimages(videopath, imagefolder, False)
#voila
print "Done"
elif len(sys.argv) == 4 and sys.argv[3] == "orb-slam2": # It has to be set up for ORB-SLAM2
#extract the arguments
videopath = sys.argv[1]
imagefolder = sys.argv[2] + "/rgb"
#run the CreateVideoBag function
CreateVideoimages(videopath, imagefolder, True)
#voila
print "Done"
else:
#The user has not priveded the right amount of arguments, point to this
print "Please supply two arguments as following: Videotofiles.py videopath imagefolder [orb-slam2] \n When adding \"orb-slam2\", the nessary text file will also be created."
Settings file
The TUM1.yaml
is copied to sam.yaml
and adjusted with the camera calibration parameters from the Samsung S5 (see LSD-Slam blog for details on calibration):
%YAML:1.0#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------# Camera calibration and distortion parameters (OpenCV)
Camera.fx: 724.6891736
Camera.fy: 726.65674145
Camera.cx: 303.71051608
Camera.cy: 204.18022629Camera.k1: 0.10975062
Camera.k2: 0.75393683
Camera.p1: -0.019259
Camera.p2: -0.00708093
Camera.k3: -5.05405739# Camera frames per second
Camera.fps: 30.0# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
Camera.RGB: 1#-----------------------------------------\S---------------------------------------------------
# ORB Parameters
#--------------------------------------------------------------------------------------------# ORB Extractor: Number of features per image
ORBextractor.nFeatures: 1000# ORB Extractor: Scale factor between levels in the scale pyramid
ORBextractor.scaleFactor: 1.2# ORB Extractor: Number of levels in the scale pyramid
ORBextractor.nLevels: 8# ORB Extractor: Fast threshold
# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response.
# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST
# You can lower these values if your images have low contrast
ORBextractor.iniThFAST: 20
ORBextractor.minThFAST: 7#--------------------------------------------------------------------------------------------
# Viewer Parameters
#--------------------------------------------------------------------------------------------
Viewer.KeyFrameSize: 0.05
Viewer.KeyFrameLineWidth: 1
Viewer.GraphLineWidth: 0.9
Viewer.PointSize:2
Viewer.CameraSize: 0.08
Viewer.CameraLineWidth: 3
Viewer.ViewpointX: 0
Viewer.ViewpointY: -0.7
Viewer.ViewpointZ: -1.8
Viewer.ViewpointF: 500
Testing on a small desk video
With the Samsung S5 camera, i captured the following video of a desk:
And after running it through Videotofiles.py, I could run the orb-slam2 that was used for the TU Munich on my own video, resulting a very nice pointcloud. An picture of the pointcloud while it was being made:
Unfortunatly, the algorithm does not let you view the pointcloud afterwards as it immediately closes the windows. So I adjusted the code of orb-slam2, see the very top for my latest modified code on github. I added some code to System.cc which saves the pointcloud in a .pcd file. Because I could not get the pcl/vdk libraries to work, (It kept complaining about missing vtk libraries that I also could not find. I tried installing from the repository and from source code but both geve the same error) So I wrote the .pcd file on my own without the libraries such that the output file would match the mark-up of .pcd files:
void System::CreatePCD(const string &filename)
{
cout << endl << "Saving map points to " << filename << " ..." << endl;vector<MapPoint*> vMPs = mpMap->GetAllMapPoints();//create pcd initialization string
std::string begin = std::string("# .PCD v.7 - Point Cloud Data file format\nVERSION .7\n");
// add the fields:
begin += "FIELDS x y z\n";
// add the size (4 for float):
begin += "SIZE 4 4 4\n";
// add the type:
begin += "TYPE F F F\n";
// add the count(ammount of dimension (1 for x)):
begin += "COUNT 1 1 1\n";
// add the width:
int width = vMPs.size();
begin += "WIDTH ";
begin += std::to_string(width);
// add the height:
begin += "\nHEIGHT 1\n";
// add the viewpoint:
begin += "VIEWPOINT 0 0 0 1 0 0 0\n";
// add the amount of points:
begin += "POINTS ";
begin += std::to_string(width);
// add the datatype:
begin += "\nDATA ascii\n";//open the file
ofstream f;
f.open(filename.c_str());
// write the begin
f << begin;//write the points
for(size_t i=0; i<vMPs.size(); i++) {
MapPoint* pMP = vMPs[i];if(pMP->isBad())
continue;cv::Mat MPPositions = pMP->GetWorldPos();f << setprecision(7)<< MPPositions.at<float>(0) << " " << MPPositions.at<float>(1) << " " << MPPositions.at<float>(2) << endl;
}f.close();
cout << endl << "Map Points saved!" << endl;
}
To be able to use this functionality, something also has to be added to System.h:
//Save the map points (Added)
void CreatePCD(const string &filename);
Then I made my own executable in the Examples folder by coping the code from mono_tum.cc to mono_rcs.cc, and adding:
// save the pointcloud
SLAM.CreatePCD("pointcloud.pcd");
To let the mono_rcs.cc be built into an .exe, CMakeLists.txt also needs to be adjusted by adding:
add_executable(mono_rcs
Examples/Monocular/mono_rcs.cc)
target_link_libraries(mono_rcs ${PROJECT_NAME} ${PCL_COMMON_LIBRARIES} ${PCL_IO_LIBRARIES})
Then it could be built (by running ./build.sh
) and now when the orb_slam2 is preformed by running:
./<path_to_mono_rcs> <path_to_vocabulary> <path_to_yaml_file> <path_to_images_folder>
The pointcloud is saved as pointcloud.pcd
, which can be viewed for instance by running: pcd_viewer <path_to_pointcloud.pcd>
. This results then in the following pointcloud for the small desk video:
Using orb-slam2 monocular on Asus Xtion camera images
Setting up the Asus Xtion
I want to write the incoming images from the Asus Xtion to an ros topic, and fortunatly their is an existing launch file that does this exactly without any other setup needed. All you have to do is install the ros-kinetic-openni2-launch
and the ros-kinetic-openni2-camera
packages. Then, which the Xtion plugged into your usb port, just run:
roslaunch openni2_launch openni2.launch
This automatically writes all kinds of data from the Xtion to a lot of topics. And so also the raw images to /camera/rgb/image_raw
and the camera calibration info to /camera/rgb/camera_info
.
With the calibration info, a Xtion_rgb.yaml
file can be creation:
%YAML:1.0#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------# Camera calibration and distortion parameters (OpenCV)
Camera.fx: 570.3422241210938
Camera.fy: 570.3422241210938
Camera.cx: 319.5
Camera.cy: 239.5Camera.k1: 0
Camera.k2: 0
Camera.p1: 0
Camera.p2: 0
Camera.k3: 0# Camera frames per second
Camera.fps: 30.0# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
Camera.RGB: 1#-----------------------------------------\S---------------------------------------------------
# ORB Parameters
#--------------------------------------------------------------------------------------------# ORB Extractor: Number of features per image
ORBextractor.nFeatures: 1000# ORB Extractor: Scale factor between levels in the scale pyramid
ORBextractor.scaleFactor: 1.2# ORB Extractor: Number of levels in the scale pyramid
ORBextractor.nLevels: 8# ORB Extractor: Fast threshold
# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response.
# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST
# You can lower these values if your images have low contrast
ORBextractor.iniThFAST: 20
ORBextractor.minThFAST: 7#--------------------------------------------------------------------------------------------
# Viewer Parameters
#--------------------------------------------------------------------------------------------
Viewer.KeyFrameSize: 0.05
Viewer.KeyFrameLineWidth: 1
Viewer.GraphLineWidth: 0.9
Viewer.PointSize:2
Viewer.CameraSize: 0.08
Viewer.CameraLineWidth: 3
Viewer.ViewpointX: 0
Viewer.ViewpointY: -0.7
Viewer.ViewpointZ: -1.8
Viewer.ViewpointF: 500
Setting up the orb_slam2 ROS-node
The example ROS node from ros_mono.cc
is already almost good to use, I have adjusted two things in the code: First of all I changed the topic it subscribed to, to /camera/rgb/image_raw
:
ros::Subscriber sub = nodeHandler.subscribe("/camera/rgb/image_raw", 1, &ImageGrabber::GrabImage,&igb);
Second I added the following lines before the ros::shutdown();
command and changed the argument check to be equal or greater than 3, so it saves the pointcloud (using the same added functionality as when using the video files from the Samsung S5 camera):
// Save camera trajectory
std::string Filename = "";
if (argc == 4){Filename = std::string(argv[3]);
std::string last = Filename.substr(Filename.length()-1);
std::string slash = "/";
if (last.compare(slash)!=0) {
Filename.push_back('/');
}
}std::string TrajectoryFilename = Filename + "KeyFrameTrajectory.txt";
SLAM.SaveKeyFrameTrajectoryTUM(TrajectoryFilename);// Save pointcloud
std::string PCDFilename = Filename + "pointcloud.pcd";
SLAM.CreatePCD(PCDFilename);
Then the example nodes can be build by going to the main orb_slam2
directory and running the following to commands (first mark build_ros.sh as executable and then execute it):
chmod +x build_ros.sh
./build_ros.sh
To be able to run the ros nodes, the ROS workspace has to be sourced by:
source <path_to_orb_slam2>/orb_slam2/Examples/ROS/ORB_SLAM2/build/devel/setup.bash
Running the orb-slam2 algorithm on the Xtion data
Now, in a sourced terminal, the orb_slam2 node can be started (make sure to have a roscore running) by running:
rosrun ORB_SLAM2 Mono <path_to_vocabulary_file> <path_to_yaml_file> [path_to_save_directory]
To start the publishing of the images, in another terminal run and move the Xtion around:
roslaunch openni2_launch openni2.launch
This way you can create a pointcloud live, for instance a pointcloud created from this scenery:
This results in the following pointcloud where you can clearly recognize the hourglass, checkerboard, notepad and keyboard:
ORB-SLAM2 on the ASUS Xtion Monocular vs RGBD
Orb-slam2 can also be used with use of the depth information from the Xtion to improve its performance. I will compare the results of the Monocular version versus the RGBD version to map out the resulting improvement in performance.
Setting up the orb_slam2 RGBD ROS-node
The RGBD ros node (ros_rgbd.cc) is almost good to use, it just needs a little code added to save the pointcloud for comparison later-on. I added the following code before and after the saving of the trajectory and altered the check on amount of arguments to check if there are 3 or more arguments:
if(argc <= 3)
{
cerr << endl << "Usage: rosrun ORB_SLAM2 RGBD path_to_vocabulary path_to_settings [path_to_save_directory]" << endl;
ros::shutdown();
return 1;
}/////////////////////////// SLAM CODE ///////////////////////////// Save camera trajectory
std::string Filename = "";
if (argc == 4){Filename = std::string(argv[3]);
std::string last = Filename.substr(Filename.length()-1);
std::string slash = "/";
if (last.compare(slash)!=0) {
Filename.push_back('/');
}
}std::string TrajectoryFilename = Filename + "KeyFrameTrajectory.txt";
SLAM.SaveKeyFrameTrajectoryTUM(TrajectoryFilename);// Save pointcloud
std::string PCDFilename = Filename + "pointcloud.pcd";
SLAM.CreatePCD(PCDFilename);
Now the rgbd orb-slam can used using the following command (after building):
rosrun ORB_SLAM2 RGBD path_to_vocabulary path_to_settings [path_to_save_directory]
Comparison between RGBD and Mono
Using rosbag, I have recorded multiple sequence of video with depth, which i will use to compare RGBD to Mono. Unfortunately, due to running out of RAM because of the difference between the writing speed vs. recording speed, some sequence turned out to skip some frames and thus experience a kind of jittering behavior. This however proved to be a good test for both to see how they deal with this.
Overall RGBD preforms better than Mono, more consistently it always immediately succeeds in initializing, while Mono usually needs a few seconds to initialize. RGBD does not loose track as fast when there are moving people in the scene as Mono and it also handles the jittering and less lighting much better as it less often looses track in these sceneries. When it does loose track, it restores more often from it by re-localizing then Mono.
So overall, the RGBD version outpreforms Mono on al lot of downsides. This is however with using a 'perfect' depthmap with a depth for each pixels. Now however, how does it preform when building thus depthmap using stixels from a 64 and 4 plane lidar?
ORB-SLAM2 on Monocular WePod images
The next step is to test it on WePod images. for this I have a sequence of 4073 images of the WePod driving outside. The calibration parameter can be filled in in the .yaml file:
%YAML:1.0#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------# Camera calibration and distortion parameters (OpenCV)
Camera.fx: 835.11736175762303
Camera.fy: 838.64392789038072
Camera.cx: 515.19367700531382
Camera.cy: 228.77263082717616Camera.k1: -0.062780912593276048
Camera.k2: 0.36742347736668979
Camera.p1: 0.00014413812511823273
Camera.p2: 0.0010414245032197963
Camera.k3: -3.0647961630801746
Camera.k4: 0.24437606952273902
Camera.k5: 0.65720812062693479
Camera.k6: -3.9822002527523273Camera.width: 1024
Camera.height: 512# Camera frames per second
Camera.fps: 15.0# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
Camera.RGB: 1#-----------------------------------------\S---------------------------------------------------
# ORB Parameters
#--------------------------------------------------------------------------------------------# ORB Extractor: Number of features per image
ORBextractor.nFeatures: 1000 # increased from 1000# ORB Extractor: Scale factor between levels in the scale pyramid
ORBextractor.scaleFactor: 1.2# ORB Extractor: Number of levels in the scale pyramid
ORBextractor.nLevels: 8 # 8# ORB Extractor: Fast threshold
# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response.
# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST
# You can lower these values if your images have low contrast
ORBextractor.iniThFAST: 20 # 20
ORBextractor.minThFAST: 7 # 7#--------------------------------------------------------------------------------------------
# Viewer Parameters
#--------------------------------------------------------------------------------------------
Viewer.KeyFrameSize: 0.05
Viewer.KeyFrameLineWidth: 1
Viewer.GraphLineWidth: 0.9
Viewer.PointSize:2
Viewer.CameraSize: 0.08
Viewer.CameraLineWidth: 3
Viewer.ViewpointX: 0
Viewer.ViewpointY: -0.7
Viewer.ViewpointZ: -1.8
Viewer.ViewpointF: 500
To create the nessesary rgb.txt file with the timestamp and corresponding images, the following python script is created:
# Create the rgb.txt file for the wepod data for orbslam
import sys, os# open the textfile
print "Opening textfile"
rgbtxtname = "rgb.txt"
textfile = open(rgbtxtname,'w')print "reading arguments"
# get the imagefolder
imagefolder = sys.argv[1]# get the fps
fps = float(sys.argv[2])print "looping over files"
# loop over all the files in the imagefolder
i=0
for root, dirs, files in os.walk(imagefolder):
files = sorted(files)
for names in files:
timestamp = i*1/fps
newline = str(timestamp) + " " + imagefolder + "/" + names + "\n"
textfile.write(newline)
i=i+1textfile.close
print "done"
And after running this, orb-slam2 can be preformed on the images. Unfortunatly it failed to initialize throughout the whole sequence. I have tried increasing the ORBextractor.nFeatures
and ORBextractor.nLevels
parameters and decreasing the ORBextractor.iniThFAST
and ORBextractor.minThFAST
parameters, without succes. I think possible causes could be a wrong calibration or the fact that almost everything in the scenery looks allot like other objects of the same type in the same image.(clouds, leaves, trees, branches, shadows of branches etc. ) This is making it hard to find the same point in the next image.