init

CMakeLists.txt

@@ -0,0 +1,147 @@
+cmake_minimum_required(VERSION 2.4.6)
+project(orb_slam2)
+
+set(ORB_SLAM2_DIR "/home/liming/ORB_SLAM2")
+set(DBOW2_DIR ${ORB_SLAM2_DIR}/Thirdparty/DBoW2)
+set(G2O_DIR ${ORB_SLAM2_DIR}/Thirdparty/g2o)
+
+# Check C++11 or C++0x support
+include(CheckCXXCompilerFlag)
+CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
+CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
+if(COMPILER_SUPPORTS_CXX11)
+   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+   add_definitions(-DCOMPILEDWITHC11)
+   message(STATUS "Using flag -std=c++11.")
+elseif(COMPILER_SUPPORTS_CXX0X)
+   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
+   add_definitions(-DCOMPILEDWITHC0X)
+   message(STATUS "Using flag -std=c++0x.")
+else()
+   message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
+endif()
+
+find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs message_generation cv_bridge image_transport tf sensor_msgs geometry_msgs genmsg)
+find_package(cmake_modules REQUIRED)
+find_package(OpenCV 2.4.3 REQUIRED)
+find_package(Eigen 3.0.0 REQUIRED)
+find_package(Pangolin REQUIRED)
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+#add_message_files(
+#  FILES
+#  IMU.msg
+#)
+
+## Generate services in the 'srv' folder
+add_service_files(
+  FILES
+  GetIMUPoseWorldService.srv
+)
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+  DEPENDENCIES
+  std_msgs  # Or other packages containing msgs
+)
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES imu_adafruit10dof
+  CATKIN_DEPENDS message_runtime cv_bridge image_transport tf geometry_msgs
+  DEPENDS Eigen
+)
+
+include_directories(
+include
+${PROJECT_SOURCE_DIR}
+${ORB_SLAM2_DIR}
+${ORB_SLAM2_DIR}/include
+${Pangolin_INCLUDE_DIRS}
+)
+
+set(LIBS 
+${catkin_LIBRARIES}
+${OpenCV_LIBS} 
+${EIGEN3_LIBS}
+${Pangolin_LIBRARIES}
+${DBOW2_DIR}/lib/libDBoW2.so
+${G2O_DIR}/lib/libg2o.so
+${ORB_SLAM2_DIR}/lib/libORB_SLAM2.so
+)
+
+# Node for monocular camera
+add_executable(Mono
+src/ros_mono.cc
+)
+
+target_link_libraries(Mono
+${LIBS}
+)
+
+
+# Node for stereo camera
+add_executable(Stereo
+src/ros_stereo.cc
+)
+
+target_link_libraries(Stereo
+${LIBS}
+)
+
+# Node for RGB-D camera
+add_executable(RGBD
+src/ros_rgbd.cc
+)
+
+target_link_libraries(RGBD
+${LIBS}
+)
+
+# Node for Get IMU world Pose
+add_executable(imu_pose_world
+    src/imu_pose_world.cc)
+
+target_link_libraries(imu_pose_world
+${LIBS}
+)

cfg/mono.yaml

@@ -0,0 +1,3 @@
+VocabularyFile: "/home/liming/ORB_SLAM2/Vocabulary/ORBvoc.bin"
+SettingFile: "/home/liming/ORB_SLAM2/Examples/Monocular/camera120.yaml"
+ImageTopic: "/image_raw"

cfg/rgbd10.yaml

@@ -0,0 +1,3 @@
+VocabularyFile: "/home/liming/ORB_SLAM2/Vocabulary/ORBvoc.bin"
+SettingFile: "/home/liming/ORB_SLAM2/Examples/RGB-D_live/kinect.yaml"
+SkipFrameNum: 2

cfg/rgbd30.yaml

@@ -0,0 +1,3 @@
+VocabularyFile: "/home/liming/ORB_SLAM2/Vocabulary/ORBvoc.bin"
+SettingFile: "/home/liming/ORB_SLAM2/Examples/RGB-D_live/kinect.yaml"
+SkipFrameNum: 0

cmake_modules/FindEigen3.cmake

@@ -0,0 +1,87 @@
+# - Try to find Eigen3 lib
+#
+# This module supports requiring a minimum version, e.g. you can do
+#   find_package(Eigen3 3.1.2)
+# to require version 3.1.2 or newer of Eigen3.
+#
+# Once done this will define
+#
+#  EIGEN3_FOUND - system has eigen lib with correct version
+#  EIGEN3_INCLUDE_DIR - the eigen include directory
+#  EIGEN3_VERSION - eigen version
+
+# Copyright (c) 2006, 2007 Montel Laurent, <[email protected]>
+# Copyright (c) 2008, 2009 Gael Guennebaud, <[email protected]>
+# Copyright (c) 2009 Benoit Jacob <[email protected]>
+# Redistribution and use is allowed according to the terms of the 2-clause BSD license.
+
+if(NOT Eigen3_FIND_VERSION)
+  if(NOT Eigen3_FIND_VERSION_MAJOR)
+    set(Eigen3_FIND_VERSION_MAJOR 2)
+  endif(NOT Eigen3_FIND_VERSION_MAJOR)
+  if(NOT Eigen3_FIND_VERSION_MINOR)
+    set(Eigen3_FIND_VERSION_MINOR 91)
+  endif(NOT Eigen3_FIND_VERSION_MINOR)
+  if(NOT Eigen3_FIND_VERSION_PATCH)
+    set(Eigen3_FIND_VERSION_PATCH 0)
+  endif(NOT Eigen3_FIND_VERSION_PATCH)
+
+  set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}")
+endif(NOT Eigen3_FIND_VERSION)
+
+macro(_eigen3_check_version)
+  file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header)
+
+  string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}")
+  set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}")
+  string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}")
+  set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}")
+  string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}")
+  set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}")
+
+  set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION})
+  if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
+    set(EIGEN3_VERSION_OK FALSE)
+  else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
+    set(EIGEN3_VERSION_OK TRUE)
+  endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
+
+  if(NOT EIGEN3_VERSION_OK)
+
+    message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, "
+                   "but at least version ${Eigen3_FIND_VERSION} is required")
+  endif(NOT EIGEN3_VERSION_OK)
+endmacro(_eigen3_check_version)
+
+if (EIGEN3_INCLUDE_DIR)
+
+  # in cache already
+  _eigen3_check_version()
+  set(EIGEN3_FOUND ${EIGEN3_VERSION_OK})
+
+else (EIGEN3_INCLUDE_DIR)
+
+  # specific additional paths for some OS
+  if (WIN32)
+    set(EIGEN_ADDITIONAL_SEARCH_PATHS ${EIGEN_ADDITIONAL_SEARCH_PATHS} "C:/Program Files/Eigen/include" "C:/Program Files (x86)/Eigen/include")
+  endif(WIN32)
+
+  find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library
+      PATHS
+      ${CMAKE_INSTALL_PREFIX}/include
+      ${EIGEN_ADDITIONAL_SEARCH_PATHS}
+      ${KDE4_INCLUDE_DIR}
+      PATH_SUFFIXES eigen3 eigen
+    )
+
+  if(EIGEN3_INCLUDE_DIR)
+    _eigen3_check_version()
+  endif(EIGEN3_INCLUDE_DIR)
+
+  include(FindPackageHandleStandardArgs)
+  find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK)
+
+  mark_as_advanced(EIGEN3_INCLUDE_DIR)
+
+endif(EIGEN3_INCLUDE_DIR)
+

launch/mono.launch

@@ -0,0 +1,4 @@
+<launch>
+  <rosparam command="load" file="$(find orb_slam2)/cfg/mono.yaml" />
+  <node pkg="orb_slam2" type="Mono" name="Mono" />
+</launch>

launch/rgbd10.launch

@@ -0,0 +1,5 @@
+<launch>
+  <node pkg="orb_slam2" type="imu_pose_world" name="imu_pose_world" />
+  <rosparam command="load" file="$(find orb_slam2)/cfg/rgbd10.yaml" />
+  <node pkg="orb_slam2" type="RGBD" name="RGBD" cwd="node"/>
+</launch>

launch/rgbd10.launch~

@@ -0,0 +1,4 @@
+<launch>
+  <rosparam command="load" file="$(find orb_slam2)/cfg/rgbd.yaml" />
+  <node pkg="orb_slam2" type="RGBD" name="RGBD" />
+</launch>

launch/rgbd30.launch

@@ -0,0 +1,4 @@
+<launch>
+  <rosparam command="load" file="$(find orb_slam2)/cfg/rgbd30.yaml" />
+  <node pkg="orb_slam2" type="RGBD" name="RGBD" />
+</launch>

launch/rgbd30.launch~

@@ -0,0 +1,4 @@
+<launch>
+  <rosparam command="load" file="$(find orb_slam2)/cfg/rgbd.yaml" />
+  <node pkg="orb_slam2" type="RGBD" name="RGBD" />
+</launch>

package.xml

@@ -0,0 +1,37 @@
+<?xml version="1.0"?>
+<package>
+  <name>orb_slam2</name>
+  <version>2.0.0</version>
+  <description>ORB_SLAM2</description>
+
+  <maintainer email="[email protected]">liming</maintainer>
+
+  <license>GPLv3</license>
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <build_depend>message_generation</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>image_transport</build_depend>
+  <build_depend>cv_bridge</build_depend>
+  <build_depend>cmake_modules</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+
+  <run_depend>message_runtime</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>tf</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>image_transport</run_depend>
+  <run_depend>cv_bridge</run_depend>
+  <run_depend>geometry_msgs</run_depend>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

src/imu_pose_world.cc

@@ -0,0 +1,70 @@
+#include "ros/ros.h"
+#include "std_msgs/String.h"
+#include "sensor_msgs/Imu.h"
+#include "tf/transform_datatypes.h"
+
+#include <sstream>
+#include <boost/function.hpp>
+#include <mutex>
+
+#include "orb_slam2/GetIMUPoseWorldService.h"
+
+/**
+ * This tutorial demonstrates simple sending of messages over the ROS system.
+ */
+ros::Publisher *pPub = NULL;
+double ax = 0, ay = 0, az = 0;
+double q0 = 1, q1 = 0, q2 = 0, q3 = 0;
+
+void GetIMUPoseWorld(const sensor_msgs::Imu &imu_data)
+{
+    ax = 0.9*ax + 0.1*imu_data.linear_acceleration.x;
+    ay = 0.9*ay + 0.1*imu_data.linear_acceleration.y;
+    az = 0.9*az + 0.1*imu_data.linear_acceleration.z;
+
+    double r = atan2(ay, az) ;
+    double p = atan2(ax, sqrt(az*az+ay*ay)) ;
+    double y = 0;
+
+    geometry_msgs::PoseStamped pose;
+    pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(r, p, y);
+    pose.header.frame_id = "world";
+
+    {
+        q0 = pose.pose.orientation.w;
+        q1 = pose.pose.orientation.x;
+        q2 = pose.pose.orientation.y;
+        q3 = pose.pose.orientation.z;
+    }
+
+    pPub->publish(pose);
+}
+
+bool GetIMUPoseWorldService(orb_slam2::GetIMUPoseWorldService::Request &req,
+        orb_slam2::GetIMUPoseWorldService::Response &res)
+{
+    res.w = q0;
+    res.x = q1;
+    res.y = q2;
+    res.z = q3;
+    return true;
+}
+
+int main(int argc, char **argv)
+{
+    // Init ros
+    ros::init(argc, argv, "imu_world_pose");
+    ros::NodeHandle n;
+
+    ros::Publisher pub = 
+        n.advertise<geometry_msgs::PoseStamped>("/imu_world_pose/pose", 5);
+    pPub = &pub;
+
+    ros::Subscriber sub = n.subscribe("/adafruit_imu/imu_data", 5, GetIMUPoseWorld);
+
+    ros::ServiceServer service = n.advertiseService("GetIMUPoseWorldService", GetIMUPoseWorldService);
+
+    ros::spin();
+
+    return 0;
+}