init GPU

2021-05-05 11:19:06 +02:00
parent 85470c37b6
commit fa3b7f1e1a
145 changed files with 37 additions and 842 deletions
@@ -0,0 +1,8 @@
+# Default ignored files
+/shelf/
+/workspace.xml
+# Datasource local storage ignored files
+/dataSources/
+/dataSources.local.xml
+# Editor-based HTTP Client requests
+/httpRequests/
@@ -0,0 +1,4 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="CMakeWorkspace" PROJECT_DIR="$PROJECT_DIR$" />
+</project>
@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="ProjectModuleManager">
+    <modules>
+      <module fileurl="file://$PROJECT_DIR$/.idea/svo_ros.iml" filepath="$PROJECT_DIR$/.idea/svo_ros.iml" />
+    </modules>
+  </component>
+</project>
@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<module classpath="CMake" type="CPP_MODULE" version="4" />
@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="VcsDirectoryMappings">
+    <mapping directory="$PROJECT_DIR$/.." vcs="Git" />
+  </component>
+</project>
@@ -0,0 +1,79 @@
+SET(PROJECT_NAME vio_svo)
+PROJECT(${PROJECT_NAME})
+CMAKE_MINIMUM_REQUIRED (VERSION 2.8.3)
+SET(CMAKE_BUILD_TYPE Release) # Release, RelWithDebInfo
+SET(CMAKE_VERBOSE_MAKEFILE OFF)
+SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PROJECT_SOURCE_DIR}/CMakeModules/")
+
+# Set build flags. Set IS_ARM on odroid board as environment variable
+SET(CMAKE_CXX_FLAGS "-Wall -D_LINUX -D_REENTRANT -march=native -Wno-unused-variable -Wno-unused-but-set-variable -Wno-unknown-pragmas")
+IF(DEFINED ENV{ARM_ARCHITECTURE})
+  SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}")
+ELSE()
+  SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mmmx -msse -msse -msse2 -msse3 -mssse3")
+ENDIF()
+IF(CMAKE_COMPILER_IS_GNUCC)
+  SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
+ELSE()
+  SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+ENDIF()
+SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops")
+
+# Add catkin and required ROS packages
+FIND_PACKAGE(catkin REQUIRED COMPONENTS
+        geometry_msgs
+        roscpp
+  cmake_modules
+  nav_msgs
+  std_msgs
+  visualization_msgs
+  sensor_msgs
+  image_transport
+  cv_bridge
+  tf
+  svo
+  svo_msgs
+  vikit_common
+  vikit_ros
+)
+
+# Add plain cmake packages 
+FIND_PACKAGE(OpenCV REQUIRED)
+FIND_PACKAGE(Eigen REQUIRED)
+FIND_PACKAGE(Sophus REQUIRED)
+FIND_PACKAGE(Boost REQUIRED COMPONENTS thread)
+FIND_PACKAGE(GTSAM REQUIRED)
+
+# Describe catkin Project
+catkin_package(
+  DEPENDS Eigen OpenCV Sophus Boost
+  CATKIN_DEPENDS roscpp nav_msgs std_msgs visualization_msgs sensor_msgs image_transport cv_bridge tf svo svo_msgs vikit_common vikit_ros geometry_msgs
+  INCLUDE_DIRS include
+  LIBRARIES svo_visualizer
+)
+
+# Include dirs
+INCLUDE_DIRECTORIES(
+        include
+  ${Eigen_INCLUDE_DIRS}
+  ${OpenCV_INCLUDE_DIRS}
+  ${Sophus_INCLUDE_DIRS}
+  ${Boost_INCLUDE_DIRS}
+  ${catkin_INCLUDE_DIRS}
+)
+
+# Set link libraries
+LIST(APPEND LINK_LIBS 
+  ${OpenCV_LIBS}
+  ${Sophus_LIBRARIES}
+  ${Boost_LIBRARIES}
+  ${catkin_LIBRARIES}
+)
+
+# Create visualizer library
+ADD_LIBRARY(svo_visualizer src/visualizer.cpp)
+TARGET_LINK_LIBRARIES(svo_visualizer ${LINK_LIBS} gtsam)
+                                 
+# Create Executables                      
+ADD_EXECUTABLE(vo src/vo_node.cpp)
+TARGET_LINK_LIBRARIES(vo svo_visualizer ${LINK_LIBS})
@@ -0,0 +1,73 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO is free software: you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the Free Software
+// Foundation, either version 3 of the License, or any later version.
+//
+// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef SVO_VISUALIZER_H_
+#define SVO_VISUALIZER_H_
+
+#include <queue>
+#include <ros/ros.h>
+#include <svo/global.h>
+#include <visualization_msgs/Marker.h>
+#include <visualization_msgs/MarkerArray.h>
+#include <tf/transform_broadcaster.h>
+#include <image_transport/image_transport.h>
+#include <boost/thread.hpp>
+#include <boost/shared_ptr.hpp>
+
+namespace svo {
+
+class Frame;
+class Point;
+class Map;
+class FrameHandlerMono;
+
+typedef boost::shared_ptr<Frame> FramePtr;
+
+/// This class bundles all functions to publish visualisation messages.
+class Visualizer
+{
+public:
+  ros::NodeHandle pnh_;
+  size_t trace_id_;
+  ros::Publisher pub_frames_;
+  ros::Publisher pub_points_;
+  ros::Publisher pub_pose_;
+  ros::Publisher pub_info_;
+  tf::TransformBroadcaster br_;
+  SE3 T_world_from_vision_;
+
+  Visualizer();
+
+  ~Visualizer() {};
+
+  void publishMinimal(
+      const cv::Mat& img,
+      const FramePtr& frame,
+      const FrameHandlerMono& slam,
+      const double timestamp);
+
+  void visualizeMarkers(
+      const FramePtr& frame,
+      const set<FramePtr>& core_kfs,
+      const Map& map);
+
+  void removeDeletedPts(const Map& map);
+
+};
+
+} // end namespace svo
+
+#endif /* SVO_VISUALIZER_H_ */
@@ -0,0 +1,22 @@
+<launch>
+
+  <node pkg="vio_svo" type="vo" name="vio" clear_params="true" output="screen">
+
+    <!-- Camera topic to subscribe to -->
+    <param name="cam_topic" value="/CoreNode/grey_img" type="str" />
+    <param name="imu_topic" value="/SensorNode/imu" type="str" />
+    
+    <!-- Camera calibration file -->
+    <rosparam file="$(find vio_svo)/param/px30.yaml" />
+
+    <!-- Initial camera orientation, make it point downwards -->
+    <param name="init_rx" value="3.14" />
+    <param name="init_ry" value="0.00" />
+    <param name="init_rz" value="0.00" />
+
+  </node>
+
+  <!-- Default parameter settings: choose between vo_fast and vo_accurate -->
+  <rosparam file="$(find vio_svo)/param/vo_fast.yaml" />
+
+</launch>
@@ -0,0 +1,16 @@
+<launch>
+  
+    <node pkg="svo_ros" type="vo" name="svo" clear_params="true" output="screen">
+    
+        <!-- Camera topic to subscribe to -->
+        <param name="cam_topic" value="/camera/image_raw" type="str" />
+        
+        <!-- Camera calibration file -->
+        <rosparam file="$(find svo_ros)/param/camera_atan.yaml" />
+        
+        <!-- Default parameter settings: choose between vo_fast and vo_accurate -->
+        <rosparam file="$(find svo_ros)/param/vo_fast.yaml" />
+
+    </node>
+        
+</launch>
@@ -0,0 +1,22 @@
+<launch>
+
+  <node pkg="vio_svo" type="vo" name="vio" clear_params="true" output="screen">
+
+    <!-- Camera topic to subscribe to -->
+    <param name="cam_topic" value="/camera/image_raw" type="str" />
+    <param name="imu_topic" value="/SensorNode/imu" type="str" />
+
+    <!-- Camera calibration file -->
+    <rosparam file="$(find vio_svo)/param/camera_atan.yaml" />
+
+    <!-- Default parameter settings: choose between vo_fast and vo_accurate -->
+    <rosparam file="$(find vio_svo)/param/vo_fast.yaml" />
+
+    <!-- Initial camera orientation, make it point downwards -->
+    <param name="init_rx" value="3.14" />
+    <param name="init_ry" value="0.00" />
+    <param name="init_rz" value="0.00" />
+
+  </node>
+
+</launch>
@@ -0,0 +1,22 @@
+<launch>
+
+  <node pkg="vio_svo" type="vo" name="vio" clear_params="true" output="screen">
+
+    <!-- Camera topic to subscribe to -->
+    <param name="cam_topic" value="/image_converter/output_image " type="str" />
+    <param name="imu_topic" value="/imu_unity/imu" type="str" />
+
+    <!-- Camera calibration file -->
+    <rosparam file="$(find vio_svo)/param/unity.yaml" />
+
+    <!-- Default parameter settings: choose between vo_fast and vo_accurate -->
+    <rosparam file="$(find vio_svo)/param/vo_fast.yaml" />
+
+    <!-- Initial camera orientation, make it point downwards -->
+    <param name="init_rx" value="0.00" />
+    <param name="init_ry" value="0.00" />
+    <param name="init_rz" value="0.00" />
+
+  </node>
+
+</launch>
@@ -0,0 +1,45 @@
+<?xml version="1.0"?>
+<package>
+  <name>vio_svo</name>
+  <version>0.1.0</version>
+  <description>
+    Monocular Visual Odometry - ROS Nodes
+  </description>
+  <author>Christian Forster</author>
+  <maintainer email="[email protected]">Christian Forster</maintainer>
+  <license>GPLv3</license>
+
+  <!-- Dependencies which this package needs to build itself. -->
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <!-- Dependencies needed to compile this package. -->
+  <build_depend>roscpp</build_depend>
+  <build_depend>cmake_modules</build_depend> <!-- for FindEigen.cmake module -->
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>visualization_msgs</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>image_transport</build_depend>
+  <build_depend>cv_bridge</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>svo</build_depend>
+  <build_depend>svo_msgs</build_depend>
+  <build_depend>vikit_common</build_depend>
+  <build_depend>vikit_ros</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  
+  <!-- Dependencies needed after this package is compiled. -->
+  <run_depend>roscpp</run_depend>
+  <run_depend>nav_msgs</run_depend>
+  <run_depend>std_msgs</run_depend>
+  <run_depend>visualization_msgs</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>image_transport</run_depend>
+  <run_depend>cv_bridge</run_depend>
+  <run_depend>tf</run_depend>
+  <run_depend>svo</run_depend>
+  <run_depend>svo_msgs</run_depend>
+  <run_depend>vikit_common</run_depend>
+  <run_depend>vikit_ros</run_depend>
+  <run_depend>geometry_msgs</run_depend>
+</package>
@@ -0,0 +1,8 @@
+cam_model: ATAN
+cam_width: 752
+cam_height: 480
+cam_fx: 0.509326
+cam_fy: 0.796651
+cam_cx: 0.45905
+cam_cy: 0.510056
+cam_d0: 0.9320
@@ -0,0 +1,11 @@
+cam_model: Pinhole
+cam_width: 640
+cam_height: 480
+cam_fx: 789.860721
+cam_fy: 790.097440
+cam_cx: 305.323524
+cam_cy: 260.464072
+cam_d0: 0.027470
+cam_d1: -0.051518
+cam_d2: 0.004375
+cam_d3: -0.004028
@@ -0,0 +1,8 @@
+cam_model: ATAN
+cam_width: 640
+cam_height: 480
+cam_fx: 0.463632
+cam_fy: 0.823661
+cam_cx: 0.497465
+cam_cy: 0.498443
+cam_d0: 1.01687
@@ -0,0 +1,8 @@
+cam_model: ATAN
+cam_width: 1024
+cam_height: 720
+cam_fx: 1.98248
+cam_fy: 2.68212
+cam_cx: 0.502524
+cam_cy: 0.497744
+cam_d0: -0.000499999
@@ -0,0 +1,4 @@
+grid_size: 20
+max_n_kfs: 50
+loba_num_iter: 10
+max_fts: 200
@@ -0,0 +1,13 @@
+svo:
+  grid_size: 30            #Feature grid size of a cell in [px].
+  max_n_kfs: 10            #Limit the number of keyframes in the map. This makes nslam essentially. a Visual Odometry. Set to 0 if unlimited number of keyframes are allowed.  Minimum number of keyframes is 3.
+  loba_num_iter: 0         #Number of iterations in the local bundle adjustment.
+  quality_min_fts: 10      #If the number of tracked features drops below this threshold. Tracking quality is bad.
+  quality_max_drop_fts: 60 #If within one frame, this amount of features are dropped. Tracking quality is bad.
+  ACC_noise: 0.00001          #acc white noise in continuous
+  GYO_noise: 0.00001          #gyro white noise in continuous
+  IUC: 0.1                #integration uncertainty continuous
+  ABC: 0.1                #acc bias in continuous
+  GBC: 0.1                #gyro bias in continuous
+  EBP: 0.1                #error in the bias used for preintegration
+  NRP: 0.0000001                #noise used for projection in preintegration
@@ -0,0 +1,293 @@
+{
+  "keys": {}, 
+  "groups": {
+    "pluginmanager": {
+      "keys": {
+        "running-plugins": {
+          "type": "repr", 
+          "repr": "{u'rqt_rviz/RViz': [1], u'rqt_reconfigure/Param': [1], u'rqt_image_view/ImageView': [1], u'svo_rqt/Svo': [1]}"
+        }
+      }, 
+      "groups": {
+        "plugin__rqt_rviz__RViz__1": {
+          "keys": {}, 
+          "groups": {
+            "dock_widget__": {
+              "keys": {
+                "dockable": {
+                  "type": "repr", 
+                  "repr": "True"
+                }, 
+                "parent": {
+                  "type": "repr", 
+                  "repr": "None"
+                }
+              }, 
+              "groups": {}
+            }
+          }
+        }, 
+        "plugin__rqt_robot_steering__RobotSteering__1": {
+          "keys": {}, 
+          "groups": {
+            "dock_widget__RobotSteeringUi": {
+              "keys": {
+                "dockable": {
+                  "type": "repr", 
+                  "repr": "u'true'"
+                }, 
+                "parent": {
+                  "type": "repr", 
+                  "repr": "None"
+                }
+              }, 
+              "groups": {}
+            }, 
+            "plugin": {
+              "keys": {
+                "topic": {
+                  "type": "repr", 
+                  "repr": "u'/cmd_vel'"
+                }, 
+                "vx_min": {
+                  "type": "repr", 
+                  "repr": "u'-1'"
+                }, 
+                "vw_min": {
+                  "type": "repr", 
+                  "repr": "u'-3'"
+                }, 
+                "vw_max": {
+                  "type": "repr", 
+                  "repr": "u'3'"
+                }, 
+                "vx_max": {
+                  "type": "repr", 
+                  "repr": "u'1'"
+                }
+              }, 
+              "groups": {}
+            }
+          }
+        }, 
+        "plugin__rqt_reconfigure__Param__1": {
+          "keys": {}, 
+          "groups": {
+            "dock_widget___plugincontainer_top_widget": {
+              "keys": {
+                "dockable": {
+                  "type": "repr", 
+                  "repr": "True"
+                }, 
+                "parent": {
+                  "type": "repr", 
+                  "repr": "None"
+                }
+              }, 
+              "groups": {}
+            }, 
+            "plugin": {
+              "keys": {
+                "splitter": {
+                  "type": "repr(QByteArray.hex)", 
+                  "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff0000000000000002000000c00000006401000000060100000001')", 
+                  "pretty-print": "         d     "
+                }, 
+                "_splitter": {
+                  "type": "repr(QByteArray.hex)", 
+                  "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff00000000000000020000013e0000006401000000090100000002')", 
+                  "pretty-print": "       > d     "
+                }
+              }, 
+              "groups": {}
+            }
+          }
+        }, 
+        "plugin__rqt_image_view__ImageView__1": {
+          "keys": {}, 
+          "groups": {
+            "dock_widget__ImageViewWidget": {
+              "keys": {
+                "dockable": {
+                  "type": "repr", 
+                  "repr": "True"
+                }, 
+                "parent": {
+                  "type": "repr", 
+                  "repr": "None"
+                }
+              }, 
+              "groups": {}
+            }, 
+            "plugin": {
+              "keys": {
+                "dynamic_range": {
+                  "type": "repr", 
+                  "repr": "False"
+                }, 
+                "max_range": {
+                  "type": "repr", 
+                  "repr": "10.0"
+                }, 
+                "topic": {
+                  "type": "repr", 
+                  "repr": "u'/svo/image'"
+                }, 
+                "zoom1": {
+                  "type": "repr", 
+                  "repr": "False"
+                }
+              }, 
+              "groups": {}
+            }
+          }
+        }, 
+        "plugin__svo_rqt__Svo__1": {
+          "keys": {}, 
+          "groups": {
+            "dock_widget__SvoWidget": {
+              "keys": {
+                "dockable": {
+                  "type": "repr", 
+                  "repr": "True"
+                }, 
+                "parent": {
+                  "type": "repr", 
+                  "repr": "None"
+                }
+              }, 
+              "groups": {}
+            }, 
+            "dock_widget__SvoUi": {
+              "keys": {
+                "dockable": {
+                  "type": "repr", 
+                  "repr": "u'true'"
+                }, 
+                "parent": {
+                  "type": "repr", 
+                  "repr": "None"
+                }
+              }, 
+              "groups": {}
+            }
+          }
+        }, 
+        "plugin__rqt_shell__Shell__1": {
+          "keys": {}, 
+          "groups": {
+            "plugin": {
+              "keys": {
+                "shell_type": {
+                  "type": "repr", 
+                  "repr": "u'0'"
+                }
+              }, 
+              "groups": {}
+            }
+          }
+        }, 
+        "plugin__rqt_console__Console__1": {
+          "keys": {}, 
+          "groups": {
+            "plugin": {
+              "keys": {
+                "show_highlighted_only": {
+                  "type": "repr", 
+                  "repr": "u'false'"
+                }, 
+                "message_limit": {
+                  "type": "repr", 
+                  "repr": "u'20000'"
+                }, 
+                "exclude_filters": {
+                  "type": "repr", 
+                  "repr": "u'severity'"
+                }, 
+                "table_splitter": {
+                  "type": "repr(QByteArray.hex)", 
+                  "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff00000000000000020000004e000000ed01000000090100000002')", 
+                  "pretty-print": "       N       "
+                }, 
+                "highlight_filters": {
+                  "type": "repr", 
+                  "repr": "u'message'"
+                }, 
+                "paused": {
+                  "type": "repr", 
+                  "repr": "u'false'"
+                }, 
+                "settings_exist": {
+                  "type": "repr", 
+                  "repr": "u'true'"
+                }, 
+                "filter_splitter": {
+                  "type": "repr(QByteArray.hex)", 
+                  "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff0000000000000002000000720000007201000000090100000002')", 
+                  "pretty-print": "       r r     "
+                }
+              }, 
+              "groups": {
+                "exclude_filter_0": {
+                  "keys": {
+                    "enabled": {
+                      "type": "repr", 
+                      "repr": "u'true'"
+                    }, 
+                    "itemlist": {
+                      "type": "repr", 
+                      "repr": "u''"
+                    }
+                  }, 
+                  "groups": {}
+                }, 
+                "highlight_filter_0": {
+                  "keys": {
+                    "regex": {
+                      "type": "repr", 
+                      "repr": "u'false'"
+                    }, 
+                    "text": {
+                      "type": "repr", 
+                      "repr": "u''"
+                    }, 
+                    "enabled": {
+                      "type": "repr", 
+                      "repr": "u'true'"
+                    }
+                  }, 
+                  "groups": {}
+                }
+              }
+            }
+          }
+        }
+      }
+    }, 
+    "mainwindow": {
+      "keys": {
+        "geometry": {
+          "type": "repr(QByteArray.hex)", 
+          "repr(QByteArray.hex)": "QtCore.QByteArray('01d9d0cb00010000000007a9fffffffc00001187000005a7000007b1000000180000117f0000059f000000000000')", 
+          "pretty-print": "                       "
+        }, 
+        "state": {
+          "type": "repr(QByteArray.hex)", 
+          "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff00000000fd0000000100000003000009cf00000559fc0100000005fb0000002c00730076006f005f007200710074005f005f00530076006f005f005f0031005f005f00530076006f005500690100000000000003890000000000000000fb0000006a007200710074005f0072006f0062006f0074005f007300740065006500720069006e0067005f005f0052006f0062006f0074005300740065006500720069006e0067005f005f0031005f005f0052006f0062006f0074005300740065006500720069006e00670055006901000000000000020b0000000000000000fc00000000000003530000022e00fffffffc0200000004fb0000003400730076006f005f007200710074005f005f00530076006f005f005f0031005f005f00530076006f0057006900640067006500740100000019000000a30000009900fffffffb0000004c007200710074005f0063006f006e0073006f006c0065005f005f0043006f006e0073006f006c0065005f005f0031005f005f0043006f006e0073006f006c006500570069006400670065007401000000c3000003b50000000000000000fb0000005a007200710074005f0069006d006100670065005f0076006900650077005f005f0049006d0061006700650056006900650077005f005f0031005f005f0049006d006100670065005600690065007700570069006400670065007401000000c2000002700000005100fffffffb0000006c007200710074005f007200650063006f006e006600690067007500720065005f005f0050006100720061006d005f005f0031005f005f005f0070006c007500670069006e0063006f006e007400610069006e00650072005f0074006f0070005f00770069006400670065007401000003380000023a0000010f00fffffffb00000026007200710074005f007200760069007a005f005f005200560069007a005f005f0031005f005f010000035900000676000000a700fffffffb00000040007200710074005f007300680065006c006c005f005f005300680065006c006c005f005f0031005f005f0058005400650072006d005700690064006700650074010000091c000000b30000000000000000000009cf0000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')", 
+          "pretty-print": "                 ,svo_rqt__Svo__1__SvoUi          jrqt_robot_steering__RobotSteering__1__RobotSteeringUi            S .                                                                                                                                              p Q                                                            8 :                           Y v                                                                                                       "
+        }
+      }, 
+      "groups": {
+        "toolbar_areas": {
+          "keys": {
+            "MinimizedDockWidgetsToolbar": {
+              "type": "repr", 
+              "repr": "8"
+            }
+          }, 
+          "groups": {}
+        }
+      }
+    }
+  }
+}
@@ -0,0 +1,153 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 108
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /TF1
+        - /Marker1
+        - /Marker2
+        - /Marker2/Namespaces1
+        - /Image1
+      Splitter Ratio: 0.5
+    Tree Height: 680
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: Image
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.03
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: rviz/TF
+      Enabled: false
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        {}
+      Update Interval: 0
+      Value: false
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: /svo/keyframes
+      Name: Marker
+      Namespaces:
+        cams: true
+        kfs: true
+      Queue Size: 100
+      Value: true
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: /svo/points
+      Name: Marker
+      Namespaces:
+        pts: true
+        trajectory: true
+      Queue Size: 100
+      Value: true
+    - Class: rviz/Image
+      Enabled: true
+      Image Topic: /svo/image
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Image
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: raw
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 255; 255; 255
+    Fixed Frame: /world
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 5.48911
+      Focal Point:
+        X: 0.777511
+        Y: 0.689927
+        Z: -0.445833
+      Name: Current View
+      Near Clip Distance: 0.01
+      Pitch: 0.125204
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 4.45215
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1416
+  Hide Left Dock: false
+  Hide Right Dock: true
+  Image:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000002cc00000542fc0200000009fb0000001200530065006c0065006300740069006f006e00000000280000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000355000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000383000001e70000001600ffffff000000010000010f000003dafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000003da000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003ce0000003efc0100000002fb0000000800540069006d00650000000000000003ce000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000006fd0000054200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 2511
+  X: 1962
+  Y: 24
@@ -0,0 +1,127 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO is free software: you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the Free Software
+// Foundation, either version 3 of the License, or any later version.
+//
+// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#include <vio_svo/visualizer.h>
+#include <svo/frame_handler_mono.h>
+#include <svo/point.h>
+#include <svo/map.h>
+#include <svo/feature.h>
+#include <svo_msgs/Info.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+#include <vikit/timer.h>
+#include <vikit/output_helper.h>
+#include <deque>
+
+namespace svo {
+
+Visualizer::
+Visualizer() :
+    pnh_("~"),
+    trace_id_(0),
+    T_world_from_vision_(Matrix3d::Identity(), Vector3d::Zero())
+{
+  // Init ROS Marker Publishers
+  pub_frames_ = pnh_.advertise<visualization_msgs::Marker>("keyframes", 10);
+  pub_points_ = pnh_.advertise<visualization_msgs::Marker>("points", 1000);
+  pub_pose_ = pnh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("pose",10);
+  pub_info_ = pnh_.advertise<svo_msgs::Info>("info", 10);
+}
+
+void Visualizer::publishMinimal(
+    const cv::Mat& img,
+    const FramePtr& frame,
+    const FrameHandlerMono& slam,
+    const double timestamp)
+{
+  ++trace_id_;
+  std_msgs::Header header_msg;
+  header_msg.frame_id = "/cam";
+  header_msg.seq = trace_id_;
+  header_msg.stamp = ros::Time(timestamp);
+
+  // publish info msg.
+  if(pub_info_.getNumSubscribers() > 0)
+  {
+    svo_msgs::Info msg_info;
+    msg_info.header = header_msg;
+    msg_info.processing_time = slam.lastProcessingTime();
+    msg_info.keyframes.reserve(slam.map().keyframes_.size());
+    for(list<FramePtr>::const_iterator it=slam.map().keyframes_.begin(); it!=slam.map().keyframes_.end(); ++it)
+      msg_info.keyframes.push_back((*it)->id_);
+    msg_info.stage = static_cast<int>(slam.stage());
+    msg_info.tracking_quality = static_cast<int>(slam.trackingQuality());
+    if(frame != NULL)
+      msg_info.num_matches = slam.lastNumObservations();
+    else
+      msg_info.num_matches = 0;
+    pub_info_.publish(msg_info);
+  }
+  if(pub_pose_.getNumSubscribers() > 0 && slam.stage() == FrameHandlerBase::STAGE_DEFAULT_FRAME)
+  {
+    Quaterniond q;
+    Vector3d p;
+    Matrix<double,6,6> Cov;
+    // publish cam in world frame (Estimated odometry in the worls frame)
+    SE3 T_world_from_cam(T_world_from_vision_*frame->T_f_w_.inverse());// do not confused with inverse. the T_f_w_ is the from camera to initial position with reference of camera
+    q = Quaterniond(T_world_from_cam.rotation_matrix()*T_world_from_vision_.rotation_matrix().transpose());
+    p = T_world_from_cam.translation();
+    Cov = T_world_from_cam.Adj()*frame->Cov_*T_world_from_cam.inverse().Adj();
+    geometry_msgs::PoseWithCovarianceStampedPtr msg_pose(new geometry_msgs::PoseWithCovarianceStamped);
+    msg_pose->header = header_msg;
+    msg_pose->pose.pose.position.x = p[0];
+    msg_pose->pose.pose.position.y = p[1];
+    msg_pose->pose.pose.position.z = p[2];
+    msg_pose->pose.pose.orientation.x = q.x();
+    msg_pose->pose.pose.orientation.y = q.y();
+    msg_pose->pose.pose.orientation.z = q.z();
+    msg_pose->pose.pose.orientation.w = q.w();
+    for(size_t i=0; i<36; ++i)
+      msg_pose->pose.covariance[i] = Cov(i%6, i/6);
+    pub_pose_.publish(msg_pose);
+  }
+}
+
+void Visualizer::visualizeMarkers(
+    const FramePtr& frame,
+    const set<FramePtr>& core_kfs,
+    const Map& map)
+{
+  if(frame == NULL)
+    return;
+
+  vk::output_helper::publishTfTransform(
+      frame->T_f_w_*T_world_from_vision_.inverse(),
+      ros::Time(frame->timestamp_), "cam_pos", "world", br_);
+
+  if(pub_frames_.getNumSubscribers() > 0 || pub_points_.getNumSubscribers() > 0)
+  {
+    vk::output_helper::publishPointMarker(
+        pub_points_, T_world_from_vision_*frame->pos(), "trajectory",
+        ros::Time::now(), trace_id_, 0, 0.006, Vector3d(0.,0.,0.5));
+    removeDeletedPts(map);
+  }
+}
+
+void Visualizer::removeDeletedPts(const Map& map)
+{
+  if(pub_points_.getNumSubscribers() > 0)
+  {
+    for(list<Point*>::const_iterator it=map.trash_points_.begin(); it!=map.trash_points_.end(); ++it)
+      vk::output_helper::publishPointMarker(pub_points_, Vector3d(), "pts", ros::Time::now(), (*it)->id_, 2, 0.006, Vector3d());
+  }
+}
+
+} // end namespace svo
@@ -0,0 +1,148 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO is free software: you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the Free Software
+// Foundation, either version 3 of the License, or any later version.
+//
+// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+#include <ros/package.h>
+#include <string>
+#include <svo/frame_handler_mono.h>
+#include <vio_svo/visualizer.h>
+#include <vikit/params_helper.h>
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/Imu.h>
+#include <image_transport/image_transport.h>
+#include <boost/thread.hpp>
+#include <cv_bridge/cv_bridge.h>
+#include <Eigen/Core>
+#include <vikit/abstract_camera.h>
+#include <vikit/camera_loader.h>
+#include <svo/frame.h>
+
+
+namespace svo {
+
+/// SVO Interface
+class VoNode
+{
+public:
+      svo::FrameHandlerMono* vo_;
+      svo::Visualizer visualizer_;
+      bool publish_markers_;                 //!< publish only the minimal amount of info (choice for embedded devices)
+      vk::AbstractCamera* cam_;
+      FILE* logtime;
+      bool quit_;
+      boost::thread* imu_th;
+      VoNode();
+      ~VoNode();
+      void imgCb(const sensor_msgs::ImageConstPtr& msg);
+      void imuCb(const sensor_msgs::ImuPtr& imu);
+      void imu();
+private:
+    double* imu_;
+};
+
+VoNode::VoNode() :
+      vo_(NULL),
+      publish_markers_(vk::getParam<bool>("vio/publish_markers", true)),
+      cam_(NULL),
+      quit_(false)
+{
+      // Create Camera
+      imu_=(double *)calloc(static_cast<std::size_t>(6), sizeof(double));
+      if(!vk::camera_loader::loadFromRosNs("vio", cam_))
+        throw std::runtime_error("Camera model not correctly specified.");
+      // Get initial position and orientation
+      visualizer_.T_world_from_vision_ = Sophus::SE3(
+          vk::rpy2dcm(Vector3d(vk::getParam<double>("vio/init_rx", 0.0),
+                               vk::getParam<double>("vio/init_ry", 0.0),
+                               vk::getParam<double>("vio/init_rz", 0.0))),
+          Eigen::Vector3d(vk::getParam<double>("vio/init_tx", 0.0),
+                          vk::getParam<double>("vio/init_ty", 0.0),
+                          vk::getParam<double>("vio/init_tz", 0.0)));
+      // Init VO and start
+      vo_ = new svo::FrameHandlerMono(cam_,visualizer_.T_world_from_vision_);
+      vo_->start();
+    imu_th=new boost::thread(&VoNode::imu,this);
+    logtime = fopen((std::string(getenv("HOME"))+"/time.txt").c_str(), "w+");
+
+}
+
+VoNode::~VoNode()
+{
+    quit_=true;
+    fclose(logtime);
+    imu_th->join();
+    delete imu_th;
+    delete logtime;
+    delete vo_;
+    delete cam_;
+}
+
+void VoNode::imgCb(const sensor_msgs::ImageConstPtr& msg)
+{
+      try {
+          auto start=std::chrono::steady_clock::now();
+          cv::Mat img=cv_bridge::toCvShare(msg, "mono8")->image;
+          vo_->addImage(img, msg->header.stamp.toSec());
+          visualizer_.publishMinimal(img, vo_->lastFrame(), *vo_, msg->header.stamp.toSec());
+          if(publish_markers_ && vo_->stage() != FrameHandlerBase::STAGE_PAUSED)
+              visualizer_.visualizeMarkers(vo_->lastFrame(), vo_->coreKeyframes(), vo_->map());
+          if(vo_->stage() == FrameHandlerMono::STAGE_PAUSED)
+              usleep(100000);
+          fprintf(logtime, "%s\n",std::to_string(std::chrono::duration<double>(std::chrono::steady_clock::now()-start).count()).c_str());
+
+      } catch (cv_bridge::Exception& e) {
+        ROS_ERROR("cv_bridge exception: %s", e.what());
+      }
+}
+void VoNode::imuCb(const sensor_msgs::ImuPtr &imu) {
+    double imu_in[6];
+    imu_in[0] = 0.1*imu_[0]+0.9*imu->linear_acceleration.x;
+    imu_in[1] = 0.1*imu_[1]+0.9*imu->linear_acceleration.y;
+    imu_in[2] = 0.1*imu_[2]+0.9*imu->linear_acceleration.z;
+    imu_in[3] = 0.1*imu_[3]+0.9*imu->angular_velocity.x;
+    imu_in[4] = 0.1*imu_[4]+0.9*imu->angular_velocity.y;
+    imu_in[5] = 0.1*imu_[5]+0.9*imu->angular_velocity.z;
+    memcpy(imu_, imu_in, static_cast<std::size_t>(6*sizeof(double)));
+    vo_->imu_integPtr_->update(imu_in);
+}
+void VoNode::imu(){
+    ros::NodeHandle nh;
+    // subscribe to cam msgs
+    image_transport::ImageTransport it(nh);
+    image_transport::Subscriber it_sub = it.subscribe(vk::getParam<std::string>("vio/cam_topic", "camera/image_raw"), 1, &svo::VoNode::imgCb, this);
+    // start processing callbacks
+    while(ros::ok() && !quit_)
+    {
+        ros::spinOnce();
+    }
+
+}
+} // namespace svo
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "vio");
+  svo::VoNode vo_node;
+  ros::NodeHandle nh;
+  ros::Subscriber imu_sub=nh.subscribe(vk::getParam<std::string>("vio/imu_topic", "imu/raw"),1,&svo::VoNode::imuCb, &vo_node);
+  // start processing callbacks
+  while(ros::ok() && !vo_node.quit_)
+  {
+      ros::spinOnce();
+      usleep(1000);
+      // TODO check when last image was processed. when too long ago. publish warning that no msgs are received!
+  }
+  printf("SVO terminated.\n");
+  return 0;
+}