Merge branch 'master' into HEAD

CMakeLists.txt

@@ -1,5 +1,5 @@
 SET(TRACE TRUE)
-SET(MONO_SLAM TRUE)
+SET(MONO_SLAM FALSE)
 SET(USE_ROS TRUE)
 
 SET(PROJECT_NAME orbslam_dwo)
@@ -51,8 +51,8 @@ find_package(Boost 1.45.0 COMPONENTS system thread REQUIRED)
 SET(vikit_common_INCLUDE_DIRS $ENV{HOME}/SVO/rpg_vikit/vikit_common/include)
 SET(vikit_common_LIBRARIES $ENV{HOME}/SVO/rpg_vikit/vikit_common/lib/libvikit_common.so)
 
-
-SET(g2o_INCLUDE_DIR $ENV{HOME}/ScaViSLAM/svslocal/include) # g2o, sophus, maybe opencv
+SET(g2o_INCLUDE_DIR $ENV{HOME}/ScaViSLAM/svslocal/include) # g2o, sophus, viso2, ceres autodiff
+SET(OPENCV_INCLUDE_DIR $ENV{HOME}/OpenCV/local_install/include) # opencv
 
 IF(USE_ROS)
   FIND_PACKAGE(OpenCV REQUIRED) # do not put findOpencv.cmake in cmake_module_path which causes an error
@@ -68,7 +68,8 @@ ELSE()
   set(OpenCV_LIBS opencv_core opencv_features2d opencv_flann opencv_gpu
                opencv_nonfree opencv_highgui opencv_imgproc opencv_calib3d opencv_video)
   SET(g2o_LIBRARY_DIR $ENV{HOME}/ScaViSLAM/svslocal/lib)
-  link_directories(${g2o_LIBRARY_DIR})
+  SET(OPENCV_LIBRARY_DIR $ENV{HOME}/OpenCV/local_install/lib)
+  link_directories(${g2o_LIBRARY_DIR} ${OPENCV_LIBRARY_DIR})
   SET(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
   SET(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/build)
 ENDIF()
@@ -81,6 +82,7 @@ ${CHOLMOD_INCLUDE_DIR}
 ${CSPARSE_INCLUDE_DIR}
 ${Boost_INCLUDE_DIRS}
 ${g2o_INCLUDE_DIR}
+${OPENCV_INCLUDE_DIR}
 ${vikit_common_INCLUDE_DIRS}
 )
 
@@ -106,25 +108,15 @@ src/Initializer.cc
 src/stereoSFM.cpp
 src/MotionModel.cpp
 src/utils.cpp
-viso2/filter.cpp
-viso2/matcher.cpp
-viso2/matrix.cpp
-#viso2/MEstimator.cpp
-viso2/p_match.h
-#viso2/timer.h
-#viso2/TrackerData.cpp
-viso2/triangle.cpp
-viso2/viso.cpp
-viso2/viso_stereo.cpp
-viso2/ptsetreg.cpp
-viso2/five-point.cpp
-g2o_types/global.h
+
 g2o_types/maths_utils.cpp
 g2o_types/eigen_utils.h
 g2o_types/eigen_utils.cpp
 g2o_types/anchored_points.cpp
 g2o_types/IMU_constraint.cpp
 g2o_types/timegrabber.cpp
+g2o_types/rand_sampler.cpp
+g2o_types/scale_solver.cpp
 )
 
 IF(USE_ROS)
@@ -146,8 +138,10 @@ ${g2o_LIBRARY_DIR}/libg2o_csparse_extension.so
 ${g2o_LIBRARY_DIR}/libg2o_types_sba.so
 ${g2o_LIBRARY_DIR}/libg2o_types_sim3.so
 ${g2o_LIBRARY_DIR}/libg2o_types_slam3d.so
+
 ${PROJECT_SOURCE_DIR}/Thirdparty/DBoW2/lib/libDBoW2.so
-${vikit_common_LIBRARIES} #for logging and timing
+$ENV{HOME}/ScaViSLAM/svslocal/lib/libviso2.so
+${vikit_common_LIBRARIES}
 )
 
 IF(USE_ROS)

README.md

@@ -1,6 +1,6 @@
 # ORB-SLAM-DWO
 
-ORB-SLAM-DWO is an adapted version of ORB-SLAM with double window optimization by Jianzhu Huai. In contrast to ORB-SLAM, (1) it does not rely on ROS, (2) it does not use the modified version of g2o shipped in ORB-SLAM, instead it used the g2o from github, (3) it used Eigen vectors and Sophus members instead of Opencv Mat to represent pose entities, and (4) it also incorporate a camera model from vikit of SVO and a motion model of Stereo PTAM for KITTI.
+ORB-SLAM-DWO is an adapted version of ORB-SLAM with double window optimization by Jianzhu Huai. In contrast to ORB-SLAM, (1) it does not rely on ROS, (2) it does not use the modified version of g2o shipped in ORB-SLAM, instead it used the g2o from github, (3) it used Eigen vectors and Sophus members instead of Opencv Mat to represent pose entities, and (4) it also incorporate a camera model from vikit of SVO and a motion model of Stereo PTAM for KITTI, (5) currently, it supported stereo SLAM, monocular SLAM, and stereo + inertial SLAM. The program may also run without ROS, but this is not well tested.
 
 ORB-SLAM is a versatile and accurate Monocular SLAM solution able to compute in real-time the camera trajectory and a sparse 3D reconstruction of the scene in a wide variety of environments, ranging from small hand-held sequences to a car driven around several city blocks. It is able to close large loops and perform global relocalisation in real-time and from wide baselines.
 
@@ -86,7 +86,7 @@ For vikit, it is not necessary to install it. But you need to set its path in th
 
 2. Clone this ORB_SLAM into your project folder
 
-3. Build DBoW2. Go into Thirdparty/DBoW2/ and execute:
+3. Build DBoW2. Go into Thirdparty/DBoW2/ and execute the following to compile DBoW2 with ROS OpenCV (if you want to build this package using independent OpenCV instead of ROS OpenCV, in line "SET(USE_ROS True)", Set False):
 
 		mkdir build
 		cd build
@@ -96,7 +96,7 @@ For vikit, it is not necessary to install it. But you need to set its path in th
 	Tip: Set your favorite compilation flags in line 4 and 5 of Thirdparty/DBoW2/CMakeLists.txt
 		  (by default -03 -march=native)
 
-4. Build ORB_SLAM. In the ORB-SLAM-DWO root execute:
+4. Build ORB_SLAM. In the ORB-SLAM-DWO root execute the following to compile with catkin (if you want to build this package without ROS, in line "SET(USE_ROS True)", Set False, but this is not well tested.) If to build this package for monocular SLAM, in line "SET(MONO_SLAM FALSE)" set MONO_SLAM true. Whether to use IMU data is determined by the use_imu_data member in the yaml setting file.
 
 		mkdir build
 		cd build
@@ -108,12 +108,14 @@ For vikit, it is not necessary to install it. But you need to set its path in th
 
 #4. Usage
 
-1. Launch ORB-SLAM-DWO from the terminal:
-
-		test_orbslam PATH_TO_SETTINGS_FILE
+1. Launch ORB-SLAM-DWO:
+	launch ROS in a terminal: roscore
+	launch image view in another terminal: rosrun image_view image_view image:=/ORBSLAM_DWO/Frame _autosize:=true
+	launch rviz in another terminal: rosrun rviz rviz -d /home/jhuai/catkin_ws/src/orbslam_dwo/data/rviz.rviz
+	and finally in another terminal: test_orbslam PATH_TO_SETTINGS_FILE
 
 	You have to provide the path to the settings file which contains path to the vocabulary file. The paths must be absolute or relative to the ORB_SLAM directory. We already provide the vocabulary file we use in ORB_SLAM/Data. Uncompress the file, as it will be loaded much faster.
-
+	
 2. The settings File
 
 	We provide the settings of example sequences in Data folder, KITTI odometry and Tsukuba CG Stereo dataset daylight.

TODO.txt

@@ -0,0 +1,18 @@
+Issues and solutions
+1. in stereo + imu and stereo configuration, testing on KITTI seq 00, the memory slowly is chipped away, and finally blocks loop closure and chokes the tracking thread
+
+This happens with static or shared libraries of viso2, debug or release mode. I suspect it is caused by too many keyframes are stored in memory. But this does not happen with monocular ORB SLAM on KITTI seq 00.
+
+2. in monocular configuration, testing on KITTI seq 00, many loop closures are not applied. 
+
+Solution: Make sure the Orbslam_dwo, DBoW2, Viso2 are linked to the same set of OpenCV libraries. In the case of using ROS, find_package(OpenCV required) in CMakeLists.txt, otherwise, add OpenCV libraries path and include path, and library names in CMakeLists.txt.
+
+3. warning sparseoptimizer::optimize " 0 vertices to optimize, maybe forgot to call initializeOptimization()" occurs during localbundleadjustment or localoptimize.
+
+This warning is not caused by running two g2o optimizer at the same time, as I tested running hundreds of threads of g2o optimizers, no such warning occurs.
+
+4. Add sigmaLevel0 for pixels at level 0
+
+5. Add covariance estimates for optimized poses
+
+6. Use anchored inverse depth parameterization for world points

Thirdparty/DBoW2/CMakeLists.txt

@@ -3,7 +3,7 @@ project(DBoW2)
 
 SET(CMAKE_BUILD_TYPE Release) # Release, RelWithDebInfo
 SET(CMAKE_VERBOSE_MAKEFILE OFF)
-SET(USE_ROS true) # Set False if you want to build this package without Catkin
+SET(USE_ROS true) # Set False if you want to build this package using independent OpenCV instead of ROS OpenCV
 set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS}  -Wall  -O3 -march=native ")
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall  -O3 -march=native")
 

Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h

@@ -1384,7 +1384,7 @@ bool TemplatedVocabulary<TDescriptor,F>::loadFromTextFile(const std::string &fil
 
         int nid = m_nodes.size();
         m_nodes.resize(m_nodes.size()+1);
-
+		m_nodes[nid].id = nid;
         int pid ;
         ssnode >> pid;
         m_nodes[nid].parent = pid;
@@ -1393,10 +1393,14 @@ bool TemplatedVocabulary<TDescriptor,F>::loadFromTextFile(const std::string &fil
         int nIsLeaf;
         ssnode >> nIsLeaf;
 
-        string sd;
+        stringstream ssd;
         for(int iD=0;iD<F::L;iD++)
-            ssnode >> sd;
-        F::fromString(m_nodes[nid].descriptor, sd);
+        {
+            string sElement;
+            ssnode >> sElement;
+            ssd << sElement << " ";
+		}
+        F::fromString(m_nodes[nid].descriptor, ssd.str());
 
         ssnode >> m_nodes[nid].weight;