Migrated latest changes from raulmur regarding vocalbulary from txt

README.md

@@ -56,18 +56,26 @@ We make use of some components of the DBoW2 library for place recognition and fe
 including only the components we need and also some modifications that are listed in `Thirdparty/DBoW2/LICENSE.txt`. 
 It only depends on OpenCV, but it should be included in the ROS distribution.
 
+##2.5 Octomap
+The integrated octomap export topics und services require following packages.
+
+	sudo apt-get install ros-indigo-octomap ros-indigo-octomap-msgs ros-indigo-octomap-ros
 
 #3. Installation
 
 1. In your ROS package path (check your environment variable `ROS_PACKAGE_PATH`) clone this repository:
 
+	Original version:
 		git clone https://github.com/raulmur/ORB_SLAM.git ORB_SLAM
+	My version:
+		git clone https://github.com/cehberlin/ORB_SLAM.git
 
 2. Run this line from your catkin workspace root, `indigo` here should be replaced with your preferred ROS distro.
 	`rosdep install --from-paths src --ignore-src --rosdistro indigo -y` 
 
 3. Build all by running catkin_make in your workspace root.
 
+	Only original version:
 	*Tip: Set your favorite compilation flags in line 12 and 13 of* `Thirdparty/DBoW2/CMakeLists.txt` (by default -03 -march=native)
 
 #4. Usage
@@ -79,7 +87,7 @@ It only depends on OpenCV, but it should be included in the ROS distribution.
 		rosrun orb_slam orb_slam PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE
 
   You have to provide the path to the ORB vocabulary and to the settings 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/ORBvoc.yml`. Uncompress the file, as it will be   loaded much faster.
+  We already provide the vocabulary file we use in `orb_slam/Data/ORBvoc.txt`. Uncompress the file, as it will be   loaded much faster.
 
 2. The last processed frame is published to the topic `/orb_slam/Frame`. You can visualize it using `image_view`:
 
@@ -124,7 +132,7 @@ We provide the settings and the rosbag of an example sequence in our lab. In thi
 
 	Uncompress the file.
 
-2. Launch ORB_SLAM with the settings for the example sequence. You should have already uncompressed the vocabulary file (`/Data/ORBvoc.yml.tar.gz`)
+2. Launch ORB_SLAM with the settings for the example sequence. You should have already uncompressed the vocabulary file (`/Data/ORBvoc.txt.tar.gz`)
 
   *in ROS Fuerte*:
 

g2o/CMakeLists.txt

@@ -109,8 +109,8 @@ IF(OPENMP_FOUND AND G2O_USE_OPENMP)
 ENDIF(OPENMP_FOUND AND G2O_USE_OPENMP)
 
 # Compiler specific options for gcc
-SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -O3 -march=native") 
-SET(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -O3 -march=native") 
+SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -O3 ") 
+SET(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -O3 ") 
 
 # activate warnings !!!
 SET(g2o_C_FLAGS "${g2o_C_FLAGS} -Wall -W")

orb_slam/CMakeLists.txt

@@ -9,15 +9,39 @@ find_package(catkin REQUIRED COMPONENTS
   image_geometry
   cv_bridge
   pcl_ros
+  pcl_conversions
   g2o
+  message_generation
 )
 
+ADD_DEFINITIONS("-std=c++11")
+
 #LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)
 #find_package(Cholmod REQUIRED)
 find_package(Eigen3 REQUIRED)
 find_package(PCL REQUIRED)
+find_package(octomap 1.6 REQUIRED)
+
+## Generate messages in the 'msg' folder
+add_message_files(
+  FILES
+  ORBState.msg
+)
+
+add_service_files(
+  FILES
+  SaveOctomap.srv
+)
 
-catkin_package()
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+)
+
+catkin_package(
+  CATKIN_DEPENDS pcl_conversions pcl_ros roscpp sensor_msgs 
+  DEPENDS message_runtime std_msgs 
+)
 
 message(*************************)
 message(${catkin_INCLUDE_DIRS})
@@ -31,10 +55,11 @@ include_directories(
   ${PCL_INCLUDE_DIRS}
   ${CHOLMOD_INCLUDE_DIR}
   ${PROJECT_SOURCE_DIR}
+  ${OCTOMAP_INCLUDE_DIRS}
 )
 
-set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS}  -Wall  -O3 -march=native ")
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall  -O3 -march=native")
+set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS}  -Wall  -O3 ")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall  -O3 ")
 
 add_subdirectory(Thirdparty/DBoW2)
 
@@ -51,16 +76,22 @@ add_executable(${PROJECT_NAME}
   src/KeyFrame.cc
   src/Map.cc
   src/MapPublisher.cc
+  src/OctoMapPublisher.cc
   src/Optimizer.cc
   src/PnPsolver.cc
   src/Frame.cc
   src/KeyFrameDatabase.cc
   src/Sim3Solver.cc
+  src/StatePublisher.cc
   src/Initializer.cc
 )
+
+add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_generate_messages_cpp)
+
 target_link_libraries(${PROJECT_NAME}
   ${catkin_LIBRARIES}
   ${EIGEN3_LIBS}
+  ${OCTOMAP_LIBRARIES}
 #  cholmod
   DBoW2
 #  g2o_core

orb_slam/Data/Settings_Morse_Kinect.yaml

@@ -0,0 +1,46 @@
+%YAML:1.0
+
+# Camera Parameters. Adjust them!
+
+# Camera frames per second
+Camera.fps: 60.0
+
+# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
+Camera.RGB: 1
+
+# Camera calibration parameters (OpenCV) 
+Camera.fx: 700
+Camera.fy: 700
+Camera.cx: 320
+Camera.cy: 240
+
+# Camera distortion paremeters (OpenCV) --
+Camera.k1: 0
+Camera.k2: 0
+Camera.p1: 0
+Camera.p2: 0
+
+# Are the images already rectified? 0=no rectified, 1=rectified
+Camera.Rectified: 0
+
+#--------------------------------------------------------------------------------------------
+### Changing the parameters below could seriously degradate the performance of the system
+
+# 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 (lower less restrictive)
+ORBextractor.fastTh: 20
+
+# ORB Extractor: Score to sort features. 0 -> Harris Score, 1 -> FAST Score
+ORBextractor.nScoreType: 1
+
+
+# Constant Velocity Motion Model (0 - disabled, 1 - enabled [recommended])
+UseMotionModel: 1

orb_slam/Data/Settings_Morse_Videocam.yaml

@@ -0,0 +1,46 @@
+%YAML:1.0
+
+# Camera Parameters. Adjust them!
+
+# 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
+
+# Camera calibration parameters (OpenCV) 
+Camera.fx: 200
+Camera.fy: 200
+Camera.cx: 320
+Camera.cy: 240
+
+# Camera distortion paremeters (OpenCV) --
+Camera.k1: 0
+Camera.k2: 0
+Camera.p1: 0
+Camera.p2: 0
+
+# Are the images already rectified? 0=no rectified, 1=rectified
+Camera.Rectified: 0
+
+#--------------------------------------------------------------------------------------------
+### Changing the parameters below could seriously degradate the performance of the system
+
+# 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 (lower less restrictive)
+ORBextractor.fastTh: 20
+
+# ORB Extractor: Score to sort features. 0 -> Harris Score, 1 -> FAST Score
+ORBextractor.nScoreType: 1
+
+
+# Constant Velocity Motion Model (0 - disabled, 1 - enabled [recommended])
+UseMotionModel: 1

orb_slam/Data/Settings_bluefox_2mm.yaml

@@ -0,0 +1,43 @@
+%YAML:1.0
+
+# Camera Parameters. Adjust them!
+
+# Camera calibration parameters (OpenCV) 
+Camera.fx: 343.994948
+Camera.fy: 343.795767
+Camera.cx: 380.994941
+Camera.cy: 259.645375
+
+# Camera distortion paremeters (OpenCV) --
+Camera.k1: -0.226983
+Camera.k2: 0.033991
+Camera.p1: 0.001966
+Camera.p2: -0.001217
+
+# 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
+
+#--------------------------------------------------------------------------------------------
+### Changing the parameters below could seriously degradate the performance of the system
+
+# 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 (lower less restrictive)			
+ORBextractor.fastTh: 10
+
+# ORB Extractor: Score to sort features. 0 -> Harris Score, 1 -> FAST Score			
+ORBextractor.nScoreType: 0
+
+
+# Constant Velocity Motion Model (0 - disabled, 1 - enabled [recommended])
+UseMotionModel: 1

orb_slam/Data/Settings_bluefox_2mm_stephan.yaml

@@ -0,0 +1,43 @@
+%YAML:1.0
+
+# Camera Parameters. Adjust them!
+
+# Camera calibration parameters (OpenCV) 
+Camera.fx: 362.731416
+Camera.fy: 363.585359
+Camera.cx: 377.087595
+Camera.cy: 273.554744
+
+# Camera distortion paremeters (OpenCV) --
+Camera.k1: -0.307447
+Camera.k2: 0.072806
+Camera.p1: 0.001345
+Camera.p2: -0.001264
+
+# 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
+
+#--------------------------------------------------------------------------------------------
+### Changing the parameters below could seriously degradate the performance of the system
+
+# 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 (lower less restrictive)			
+ORBextractor.fastTh: 10
+
+# ORB Extractor: Score to sort features. 0 -> Harris Score, 1 -> FAST Score			
+ORBextractor.nScoreType: 0
+
+
+# Constant Velocity Motion Model (0 - disabled, 1 - enabled [recommended])
+UseMotionModel: 1

orb_slam/Data/Settings_bluefox_6mm.yaml

@@ -0,0 +1,43 @@
+%YAML:1.0
+
+# Camera Parameters. Adjust them!
+
+# Camera calibration parameters (OpenCV) 
+Camera.fx: 1014.270444
+Camera.fy: 1011.963701
+Camera.cx: 365.890443
+Camera.cy: 278.257872
+
+# Camera distortion paremeters (OpenCV) --
+Camera.k1: -0.611255
+Camera.k2: 0.427913
+Camera.p1: -0.001147
+Camera.p2: 0.000403
+
+# 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
+
+#--------------------------------------------------------------------------------------------
+### Changing the parameters below could seriously degradate the performance of the system
+
+# 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 (lower less restrictive)			
+ORBextractor.fastTh: 10
+
+# ORB Extractor: Score to sort features. 0 -> Harris Score, 1 -> FAST Score			
+ORBextractor.nScoreType: 0
+
+
+# Constant Velocity Motion Model (0 - disabled, 1 - enabled [recommended])
+UseMotionModel: 1