add imu sub

2021-04-07 19:19:35 +02:00
parent e9256c9903
commit 651dcdfb2f
37 changed files with 786 additions and 310 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.iml" filepath="$PROJECT_DIR$/.idea/svo.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>
@@ -156,7 +156,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
    return RESULT_FAILURE;
  }

-  // pose optimization
+  // pose optimization subject to change for adding IMU
  SVO_START_TIMER("pose_optimizer");
  size_t sfba_n_edges_final;
  double sfba_thresh, sfba_error_init, sfba_error_final;
@@ -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_msgs.iml" filepath="$PROJECT_DIR$/.idea/svo_msgs.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,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,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<module type="CPP_MODULE" version="4">
+  <component name="NewModuleRootManager">
+    <content url="file://$MODULE_DIR$" />
+    <orderEntry type="inheritedJdk" />
+    <orderEntry type="sourceFolder" forTests="false" />
+  </component>
+</module>
@@ -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,627 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="CMakeRunConfigurationManager" shouldGenerate="true" shouldDeleteObsolete="true">
+    <generated>
+      <config projectName="vio_svo" targetName="actionlib_msgs_generate_messages_lisp" />
+      <config projectName="vio_svo" targetName="nav_msgs_generate_messages_cpp" />
+      <config projectName="vio_svo" targetName="visualization_msgs_generate_messages_eus" />
+      <config projectName="vio_svo" targetName="vo" />
+      <config projectName="vio_svo" targetName="actionlib_generate_messages_py" />
+      <config projectName="vio_svo" targetName="geometry_msgs_generate_messages_eus" />
+      <config projectName="vio_svo" targetName="rosgraph_msgs_generate_messages_cpp" />
+      <config projectName="vio_svo" targetName="roscpp_generate_messages_nodejs" />
+      <config projectName="vio_svo" targetName="rosgraph_msgs_generate_messages_lisp" />
+      <config projectName="vio_svo" targetName="svo_msgs_generate_messages_eus" />
+      <config projectName="vio_svo" targetName="svo_msgs_generate_messages_py" />
+      <config projectName="vio_svo" targetName="std_msgs_generate_messages_py" />
+      <config projectName="vio_svo" targetName="tf_generate_messages_eus" />
+      <config projectName="vio_svo" targetName="tf2_msgs_generate_messages_cpp" />
+      <config projectName="vio_svo" targetName="doxygen" />
+      <config projectName="vio_svo" targetName="nav_msgs_generate_messages_lisp" />
+      <config projectName="vio_svo" targetName="svo_visualizer" />
+      <config projectName="vio_svo" targetName="std_msgs_generate_messages_cpp" />
+      <config projectName="vio_svo" targetName="visualization_msgs_generate_messages_cpp" />
+      <config projectName="vio_svo" targetName="nav_msgs_generate_messages_eus" />
+      <config projectName="vio_svo" targetName="gtest" />
+      <config projectName="vio_svo" targetName="std_msgs_generate_messages_nodejs" />
+      <config projectName="vio_svo" targetName="geometry_msgs_generate_messages_cpp" />
+      <config projectName="vio_svo" targetName="nav_msgs_generate_messages_py" />
+      <config projectName="vio_svo" targetName="tf2_msgs_generate_messages_py" />
+      <config projectName="vio_svo" targetName="actionlib_generate_messages_eus" />
+      <config projectName="vio_svo" targetName="rosgraph_msgs_generate_messages_eus" />
+      <config projectName="vio_svo" targetName="clean_test_results" />
+      <config projectName="vio_svo" targetName="svo_msgs_generate_messages_cpp" />
+      <config projectName="vio_svo" targetName="svo_msgs_generate_messages_nodejs" />
+      <config projectName="vio_svo" targetName="visualization_msgs_generate_messages_nodejs" />
+      <config projectName="vio_svo" targetName="roscpp_generate_messages_py" />
+      <config projectName="vio_svo" targetName="rosgraph_msgs_generate_messages_py" />
+      <config projectName="vio_svo" targetName="tf_generate_messages_cpp" />
+      <config projectName="vio_svo" targetName="tf2_msgs_generate_messages_eus" />
+      <config projectName="vio_svo" targetName="gmock" />
+      <config projectName="vio_svo" targetName="actionlib_generate_messages_nodejs" />
+      <config projectName="vio_svo" targetName="std_msgs_generate_messages_eus" />
+      <config projectName="vio_svo" targetName="svo_msgs_generate_messages_lisp" />
+      <config projectName="vio_svo" targetName="tf2_msgs_generate_messages_lisp" />
+      <config projectName="vio_svo" targetName="std_msgs_generate_messages_lisp" />
+      <config projectName="vio_svo" targetName="tf2_msgs_generate_messages_nodejs" />
+      <config projectName="vio_svo" targetName="sensor_msgs_generate_messages_lisp" />
+      <config projectName="vio_svo" targetName="rosgraph_msgs_generate_messages_nodejs" />
+      <config projectName="vio_svo" targetName="actionlib_generate_messages_cpp" />
+      <config projectName="vio_svo" targetName="tf_generate_messages_py" />
+      <config projectName="vio_svo" targetName="visualization_msgs_generate_messages_py" />
+      <config projectName="vio_svo" targetName="tf_generate_messages_nodejs" />
+      <config projectName="vio_svo" targetName="download_extra_data" />
+      <config projectName="vio_svo" targetName="geometry_msgs_generate_messages_lisp" />
+      <config projectName="vio_svo" targetName="gtest_main" />
+      <config projectName="vio_svo" targetName="run_tests" />
+      <config projectName="vio_svo" targetName="geometry_msgs_generate_messages_nodejs" />
+      <config projectName="vio_svo" targetName="geometry_msgs_generate_messages_py" />
+      <config projectName="vio_svo" targetName="actionlib_generate_messages_lisp" />
+      <config projectName="vio_svo" targetName="actionlib_msgs_generate_messages_eus" />
+      <config projectName="vio_svo" targetName="roscpp_generate_messages_eus" />
+      <config projectName="vio_svo" targetName="nav_msgs_generate_messages_nodejs" />
+      <config projectName="vio_svo" targetName="actionlib_msgs_generate_messages_nodejs" />
+      <config projectName="vio_svo" targetName="tests" />
+      <config projectName="vio_svo" targetName="actionlib_msgs_generate_messages_py" />
+      <config projectName="vio_svo" targetName="sensor_msgs_generate_messages_eus" />
+      <config projectName="vio_svo" targetName="roscpp_generate_messages_lisp" />
+      <config projectName="vio_svo" targetName="visualization_msgs_generate_messages_lisp" />
+      <config projectName="vio_svo" targetName="gmock_main" />
+      <config projectName="vio_svo" targetName="sensor_msgs_generate_messages_py" />
+      <config projectName="vio_svo" targetName="tf_generate_messages_lisp" />
+      <config projectName="vio_svo" targetName="actionlib_msgs_generate_messages_cpp" />
+      <config projectName="vio_svo" targetName="roscpp_generate_messages_cpp" />
+      <config projectName="vio_svo" targetName="sensor_msgs_generate_messages_nodejs" />
+      <config projectName="vio_svo" targetName="sensor_msgs_generate_messages_cpp" />
+    </generated>
+  </component>
+  <component name="CMakeSettings">
+    <configurations>
+      <configuration PROFILE_NAME="Debug" CONFIG_NAME="Debug" GENERATION_DIR="$PROJECT_DIR$/../../../build/vio_svo" ENABLED="true" />
+    </configurations>
+  </component>
+  <component name="ChangeListManager">
+    <list default="true" id="1039b41d-6e4f-44ce-b5bc-8d34c33b3920" name="Default Changelist" comment="optimize the visualizer">
+      <change beforePath="$PROJECT_DIR$/../svo/src/frame_handler_mono.cpp" beforeDir="false" afterPath="$PROJECT_DIR$/../svo/src/frame_handler_mono.cpp" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/CMakeLists.txt" beforeDir="false" afterPath="$PROJECT_DIR$/../vio_svo/CMakeLists.txt" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/include/svo_ros/dataset_img.h" beforeDir="false" afterPath="$PROJECT_DIR$/../vio_svo/include/vio_svo/dataset_img.h" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/include/svo_ros/visualizer.h" beforeDir="false" afterPath="$PROJECT_DIR$/../vio_svo/include/vio_svo/visualizer.h" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/launch/Px30.launch" beforeDir="false" afterPath="$PROJECT_DIR$/../vio_svo/launch/Px30.launch" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/launch/live.launch" beforeDir="false" afterPath="$PROJECT_DIR$/../vio_svo/launch/live.launch" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/launch/test.launch" beforeDir="false" afterPath="$PROJECT_DIR$/../vio_svo/launch/test.launch" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/package.xml" beforeDir="false" afterPath="$PROJECT_DIR$/../vio_svo/package.xml" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/param/camera_atan.yaml" beforeDir="false" afterPath="$PROJECT_DIR$/../vio_svo/param/camera_atan.yaml" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/param/camera_pinhole.yaml" beforeDir="false" afterPath="$PROJECT_DIR$/../vio_svo/param/camera_pinhole.yaml" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/param/px30.yaml" beforeDir="false" afterPath="$PROJECT_DIR$/../vio_svo/param/px30.yaml" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/param/vo_accurate.yaml" beforeDir="false" afterPath="$PROJECT_DIR$/../vio_svo/param/vo_accurate.yaml" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/param/vo_fast.yaml" beforeDir="false" afterPath="$PROJECT_DIR$/../vio_svo/param/vo_fast.yaml" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/rqt.perspective" beforeDir="false" afterPath="$PROJECT_DIR$/../vio_svo/rqt.perspective" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/rviz_config.rviz" beforeDir="false" afterPath="$PROJECT_DIR$/../vio_svo/rviz_config.rviz" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/src/benchmark_node.cpp" beforeDir="false" />
+      <change beforePath="$PROJECT_DIR$/src/visualizer.cpp" beforeDir="false" afterPath="$PROJECT_DIR$/../vio_svo/src/visualizer.cpp" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/src/vo_node.cpp" beforeDir="false" afterPath="$PROJECT_DIR$/../vio_svo/src/vo_node.cpp" afterDir="false" />
+    </list>
+    <option name="SHOW_DIALOG" value="false" />
+    <option name="HIGHLIGHT_CONFLICTS" value="true" />
+    <option name="HIGHLIGHT_NON_ACTIVE_CHANGELIST" value="false" />
+    <option name="LAST_RESOLUTION" value="IGNORE" />
+  </component>
+  <component name="ClangdSettings">
+    <option name="formatViaClangd" value="false" />
+  </component>
+  <component name="Git.Settings">
+    <option name="RECENT_GIT_ROOT_PATH" value="$PROJECT_DIR$/.." />
+  </component>
+  <component name="ProjectId" id="1psbIlZyJHxcaspyH2BlpVDsNWt" />
+  <component name="ProjectLevelVcsManager" settingsEditedManually="true" />
+  <component name="ProjectViewState">
+    <option name="hideEmptyMiddlePackages" value="true" />
+    <option name="showLibraryContents" value="true" />
+  </component>
+  <component name="PropertiesComponent">
+    <property name="RunOnceActivity.OpenProjectViewOnStart" value="true" />
+    <property name="RunOnceActivity.ShowReadmeOnStart" value="true" />
+    <property name="WebServerToolWindowFactoryState" value="false" />
+    <property name="cf.first.check.clang-format" value="false" />
+    <property name="last_opened_file_path" value="$PROJECT_DIR$/../vio_svo/CMakeLists.txt" />
+    <property name="node.js.detected.package.eslint" value="true" />
+    <property name="node.js.detected.package.tslint" value="true" />
+    <property name="node.js.path.for.package.eslint" value="project" />
+    <property name="node.js.path.for.package.tslint" value="project" />
+    <property name="node.js.selected.package.eslint" value="(autodetect)" />
+    <property name="node.js.selected.package.tslint" value="(autodetect)" />
+    <property name="nodejs_package_manager_path" value="npm" />
+    <property name="settings.editor.selected.configurable" value="CMakeSettings" />
+  </component>
+  <component name="RunManager" selected="CMake Application.vo">
+    <configuration name="actionlib_generate_messages_cpp" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="actionlib_generate_messages_cpp" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="actionlib_generate_messages_eus" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="actionlib_generate_messages_eus" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="actionlib_generate_messages_lisp" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="actionlib_generate_messages_lisp" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="actionlib_generate_messages_nodejs" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="actionlib_generate_messages_nodejs" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="actionlib_generate_messages_py" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="actionlib_generate_messages_py" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="actionlib_msgs_generate_messages_cpp" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="actionlib_msgs_generate_messages_cpp" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="actionlib_msgs_generate_messages_eus" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="actionlib_msgs_generate_messages_eus" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="actionlib_msgs_generate_messages_lisp" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="actionlib_msgs_generate_messages_lisp" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="actionlib_msgs_generate_messages_nodejs" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="actionlib_msgs_generate_messages_nodejs" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="actionlib_msgs_generate_messages_py" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="actionlib_msgs_generate_messages_py" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="clean_test_results" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="clean_test_results" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="download_extra_data" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="download_extra_data" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="doxygen" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="doxygen" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="geometry_msgs_generate_messages_cpp" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="geometry_msgs_generate_messages_cpp" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="geometry_msgs_generate_messages_eus" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="geometry_msgs_generate_messages_eus" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="geometry_msgs_generate_messages_lisp" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="geometry_msgs_generate_messages_lisp" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="geometry_msgs_generate_messages_nodejs" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="geometry_msgs_generate_messages_nodejs" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="geometry_msgs_generate_messages_py" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="geometry_msgs_generate_messages_py" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="gmock" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="gmock" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="gmock_main" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="gmock_main" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="gtest" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="gtest" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="gtest_main" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="gtest_main" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="nav_msgs_generate_messages_cpp" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="nav_msgs_generate_messages_cpp" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="nav_msgs_generate_messages_eus" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="nav_msgs_generate_messages_eus" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="nav_msgs_generate_messages_lisp" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="nav_msgs_generate_messages_lisp" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="nav_msgs_generate_messages_nodejs" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="nav_msgs_generate_messages_nodejs" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="nav_msgs_generate_messages_py" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="nav_msgs_generate_messages_py" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="roscpp_generate_messages_cpp" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="roscpp_generate_messages_cpp" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="roscpp_generate_messages_eus" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="roscpp_generate_messages_eus" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="roscpp_generate_messages_lisp" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="roscpp_generate_messages_lisp" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="roscpp_generate_messages_nodejs" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="roscpp_generate_messages_nodejs" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="roscpp_generate_messages_py" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="roscpp_generate_messages_py" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="rosgraph_msgs_generate_messages_cpp" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="rosgraph_msgs_generate_messages_cpp" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="rosgraph_msgs_generate_messages_eus" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="rosgraph_msgs_generate_messages_eus" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="rosgraph_msgs_generate_messages_lisp" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="rosgraph_msgs_generate_messages_lisp" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="rosgraph_msgs_generate_messages_nodejs" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="rosgraph_msgs_generate_messages_nodejs" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="rosgraph_msgs_generate_messages_py" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="rosgraph_msgs_generate_messages_py" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="run_tests" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="run_tests" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="sensor_msgs_generate_messages_cpp" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="sensor_msgs_generate_messages_cpp" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="sensor_msgs_generate_messages_eus" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="sensor_msgs_generate_messages_eus" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="sensor_msgs_generate_messages_lisp" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="sensor_msgs_generate_messages_lisp" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="sensor_msgs_generate_messages_nodejs" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="sensor_msgs_generate_messages_nodejs" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="sensor_msgs_generate_messages_py" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="sensor_msgs_generate_messages_py" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="std_msgs_generate_messages_cpp" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="std_msgs_generate_messages_cpp" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="std_msgs_generate_messages_eus" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="std_msgs_generate_messages_eus" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="std_msgs_generate_messages_lisp" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="std_msgs_generate_messages_lisp" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="std_msgs_generate_messages_nodejs" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="std_msgs_generate_messages_nodejs" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="std_msgs_generate_messages_py" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="std_msgs_generate_messages_py" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="svo_msgs_generate_messages_cpp" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="svo_msgs_generate_messages_cpp" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="svo_msgs_generate_messages_eus" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="svo_msgs_generate_messages_eus" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="svo_msgs_generate_messages_lisp" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="svo_msgs_generate_messages_lisp" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="svo_msgs_generate_messages_nodejs" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="svo_msgs_generate_messages_nodejs" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="svo_msgs_generate_messages_py" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="svo_msgs_generate_messages_py" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="svo_visualizer" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="svo_visualizer" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="tests" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="tests" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="tf2_msgs_generate_messages_cpp" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="tf2_msgs_generate_messages_cpp" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="tf2_msgs_generate_messages_eus" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="tf2_msgs_generate_messages_eus" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="tf2_msgs_generate_messages_lisp" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="tf2_msgs_generate_messages_lisp" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="tf2_msgs_generate_messages_nodejs" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="tf2_msgs_generate_messages_nodejs" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="tf2_msgs_generate_messages_py" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="tf2_msgs_generate_messages_py" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="tf_generate_messages_cpp" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="tf_generate_messages_cpp" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="tf_generate_messages_eus" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="tf_generate_messages_eus" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="tf_generate_messages_lisp" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="tf_generate_messages_lisp" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="tf_generate_messages_nodejs" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="tf_generate_messages_nodejs" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="tf_generate_messages_py" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="tf_generate_messages_py" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="visualization_msgs_generate_messages_cpp" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="visualization_msgs_generate_messages_cpp" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="visualization_msgs_generate_messages_eus" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="visualization_msgs_generate_messages_eus" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="visualization_msgs_generate_messages_lisp" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="visualization_msgs_generate_messages_lisp" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="visualization_msgs_generate_messages_nodejs" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="visualization_msgs_generate_messages_nodejs" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="visualization_msgs_generate_messages_py" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="visualization_msgs_generate_messages_py" CONFIG_NAME="Debug">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration name="vo" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="vio_svo" TARGET_NAME="vo" CONFIG_NAME="Debug" RUN_TARGET_PROJECT_NAME="vio_svo" RUN_TARGET_NAME="vo">
+      <method v="2">
+        <option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <configuration default="true" type="GradleAppRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" PASS_PARENT_ENVS_2="true">
+      <method v="2">
+        <option name="com.jetbrains.cidr.cpp.gradle.execution.GradleNativeBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
+      </method>
+    </configuration>
+    <list>
+      <item itemvalue="CMake Application.actionlib_generate_messages_cpp" />
+      <item itemvalue="CMake Application.actionlib_generate_messages_eus" />
+      <item itemvalue="CMake Application.actionlib_generate_messages_lisp" />
+      <item itemvalue="CMake Application.actionlib_generate_messages_nodejs" />
+      <item itemvalue="CMake Application.actionlib_generate_messages_py" />
+      <item itemvalue="CMake Application.actionlib_msgs_generate_messages_cpp" />
+      <item itemvalue="CMake Application.actionlib_msgs_generate_messages_eus" />
+      <item itemvalue="CMake Application.actionlib_msgs_generate_messages_lisp" />
+      <item itemvalue="CMake Application.actionlib_msgs_generate_messages_nodejs" />
+      <item itemvalue="CMake Application.actionlib_msgs_generate_messages_py" />
+      <item itemvalue="CMake Application.clean_test_results" />
+      <item itemvalue="CMake Application.download_extra_data" />
+      <item itemvalue="CMake Application.doxygen" />
+      <item itemvalue="CMake Application.geometry_msgs_generate_messages_cpp" />
+      <item itemvalue="CMake Application.geometry_msgs_generate_messages_eus" />
+      <item itemvalue="CMake Application.geometry_msgs_generate_messages_lisp" />
+      <item itemvalue="CMake Application.geometry_msgs_generate_messages_nodejs" />
+      <item itemvalue="CMake Application.geometry_msgs_generate_messages_py" />
+      <item itemvalue="CMake Application.nav_msgs_generate_messages_cpp" />
+      <item itemvalue="CMake Application.nav_msgs_generate_messages_eus" />
+      <item itemvalue="CMake Application.nav_msgs_generate_messages_lisp" />
+      <item itemvalue="CMake Application.nav_msgs_generate_messages_nodejs" />
+      <item itemvalue="CMake Application.nav_msgs_generate_messages_py" />
+      <item itemvalue="CMake Application.roscpp_generate_messages_cpp" />
+      <item itemvalue="CMake Application.roscpp_generate_messages_eus" />
+      <item itemvalue="CMake Application.roscpp_generate_messages_lisp" />
+      <item itemvalue="CMake Application.roscpp_generate_messages_nodejs" />
+      <item itemvalue="CMake Application.roscpp_generate_messages_py" />
+      <item itemvalue="CMake Application.rosgraph_msgs_generate_messages_cpp" />
+      <item itemvalue="CMake Application.rosgraph_msgs_generate_messages_eus" />
+      <item itemvalue="CMake Application.rosgraph_msgs_generate_messages_lisp" />
+      <item itemvalue="CMake Application.rosgraph_msgs_generate_messages_nodejs" />
+      <item itemvalue="CMake Application.rosgraph_msgs_generate_messages_py" />
+      <item itemvalue="CMake Application.run_tests" />
+      <item itemvalue="CMake Application.sensor_msgs_generate_messages_cpp" />
+      <item itemvalue="CMake Application.sensor_msgs_generate_messages_eus" />
+      <item itemvalue="CMake Application.sensor_msgs_generate_messages_lisp" />
+      <item itemvalue="CMake Application.sensor_msgs_generate_messages_nodejs" />
+      <item itemvalue="CMake Application.sensor_msgs_generate_messages_py" />
+      <item itemvalue="CMake Application.std_msgs_generate_messages_cpp" />
+      <item itemvalue="CMake Application.std_msgs_generate_messages_eus" />
+      <item itemvalue="CMake Application.std_msgs_generate_messages_lisp" />
+      <item itemvalue="CMake Application.std_msgs_generate_messages_nodejs" />
+      <item itemvalue="CMake Application.std_msgs_generate_messages_py" />
+      <item itemvalue="CMake Application.svo_msgs_generate_messages_cpp" />
+      <item itemvalue="CMake Application.svo_msgs_generate_messages_eus" />
+      <item itemvalue="CMake Application.svo_msgs_generate_messages_lisp" />
+      <item itemvalue="CMake Application.svo_msgs_generate_messages_nodejs" />
+      <item itemvalue="CMake Application.svo_msgs_generate_messages_py" />
+      <item itemvalue="CMake Application.svo_visualizer" />
+      <item itemvalue="CMake Application.tests" />
+      <item itemvalue="CMake Application.tf2_msgs_generate_messages_cpp" />
+      <item itemvalue="CMake Application.tf2_msgs_generate_messages_eus" />
+      <item itemvalue="CMake Application.tf2_msgs_generate_messages_lisp" />
+      <item itemvalue="CMake Application.tf2_msgs_generate_messages_nodejs" />
+      <item itemvalue="CMake Application.tf2_msgs_generate_messages_py" />
+      <item itemvalue="CMake Application.tf_generate_messages_cpp" />
+      <item itemvalue="CMake Application.tf_generate_messages_eus" />
+      <item itemvalue="CMake Application.tf_generate_messages_lisp" />
+      <item itemvalue="CMake Application.tf_generate_messages_nodejs" />
+      <item itemvalue="CMake Application.tf_generate_messages_py" />
+      <item itemvalue="CMake Application.visualization_msgs_generate_messages_cpp" />
+      <item itemvalue="CMake Application.visualization_msgs_generate_messages_eus" />
+      <item itemvalue="CMake Application.visualization_msgs_generate_messages_lisp" />
+      <item itemvalue="CMake Application.visualization_msgs_generate_messages_nodejs" />
+      <item itemvalue="CMake Application.visualization_msgs_generate_messages_py" />
+      <item itemvalue="CMake Application.vo" />
+      <item itemvalue="CMake Application.gmock" />
+      <item itemvalue="CMake Application.gmock_main" />
+      <item itemvalue="CMake Application.gtest" />
+      <item itemvalue="CMake Application.gtest_main" />
+    </list>
+  </component>
+  <component name="SpellCheckerSettings" RuntimeDictionaries="0" Folders="0" CustomDictionaries="0" DefaultDictionary="application-level" UseSingleDictionary="true" transferred="true" />
+  <component name="TaskManager">
+    <task active="true" id="Default" summary="Default task">
+      <changelist id="1039b41d-6e4f-44ce-b5bc-8d34c33b3920" name="Default Changelist" comment="" />
+      <created>1615974669888</created>
+      <option name="number" value="Default" />
+      <option name="presentableId" value="Default" />
+      <updated>1615974669888</updated>
+      <workItem from="1615974672234" duration="4403000" />
+      <workItem from="1617031785627" duration="1205000" />
+      <workItem from="1617034041379" duration="939000" />
+      <workItem from="1617180383827" duration="11182000" />
+      <workItem from="1617259776243" duration="13999000" />
+      <workItem from="1617360128932" duration="77000" />
+      <workItem from="1617628276956" duration="5665000" />
+      <workItem from="1617634008721" duration="6021000" />
+      <workItem from="1617703114896" duration="3471000" />
+      <workItem from="1617780607861" duration="29000" />
+    </task>
+    <task id="LOCAL-00001" summary="fix bug 238">
+      <created>1617193017745</created>
+      <option name="number" value="00001" />
+      <option name="presentableId" value="LOCAL-00001" />
+      <option name="project" value="LOCAL" />
+      <updated>1617193017745</updated>
+    </task>
+    <task id="LOCAL-00002" summary="optimize the visualizer">
+      <created>1617640897291</created>
+      <option name="number" value="00002" />
+      <option name="presentableId" value="LOCAL-00002" />
+      <option name="project" value="LOCAL" />
+      <updated>1617640897291</updated>
+    </task>
+    <option name="localTasksCounter" value="3" />
+    <servers />
+  </component>
+  <component name="TypeScriptGeneratedFilesManager">
+    <option name="version" value="3" />
+  </component>
+  <component name="Vcs.Log.Tabs.Properties">
+    <option name="TAB_STATES">
+      <map>
+        <entry key="MAIN">
+          <value>
+            <State />
+          </value>
+        </entry>
+      </map>
+    </option>
+    <option name="oldMeFiltersMigrated" value="true" />
+  </component>
+  <component name="VcsManagerConfiguration">
+    <MESSAGE value="fix bug 238" />
+    <MESSAGE value="optimize the visualizer" />
+    <option name="LAST_COMMIT_MESSAGE" value="optimize the visualizer" />
+  </component>
+</project>
@@ -1,272 +0,0 @@
-// 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 <vector>
-#include <string>
-#include <iostream>
-#include <sophus/se3.h>
-#include <ros/ros.h>
-#include <ros/package.h>
-#include <opencv2/opencv.hpp>
-#include <vikit/file_reader.h>
-#include <vikit/params_helper.h>
-#include <vikit/camera_loader.h>
-#include <vikit/abstract_camera.h>
-#include <vikit/blender_utils.h>
-#include <vikit/sample.h>
-#include <svo/config.h>
-#include <svo/frame.h>
-#include <svo/feature.h>
-#include <svo/map.h>
-#include <svo/point.h>
-#include <svo/frame_handler_mono.h>
-#include <svo/feature_detection.h>
-#include <svo_ros/visualizer.h>
-#include <svo_ros/dataset_img.h>
-
-namespace svo {
-
-class BenchmarkNode
-{
-  svo::FrameHandlerMono* vo_;
-  svo::Visualizer visualizer_;
-  int frame_count_;
-  std::ofstream trace_est_pose_;
-  std::ofstream trace_trans_error_;
-  std::ofstream trace_rot_error_;
-  std::ofstream trace_depth_error_;
-  vk::AbstractCamera* cam_;
-  double img_noise_sigma_;
-
-public:
-  BenchmarkNode(ros::NodeHandle& nh);
-  ~BenchmarkNode();
-  void tracePose(const SE3& T_w_f, const double timestamp);
-  void tracePoseError(const SE3& T_f_gt, const double timestamp);
-  void traceDepthError(const FramePtr& frame, const cv::Mat& depthmap);
-  void addNoiseToImage(cv::Mat& img, double sigma);
-  void runBenchmark(const std::string& dataset_dir);
-  void runBlenderBenchmark(const std::string& dataset_dir);
-};
-
-BenchmarkNode::BenchmarkNode(ros::NodeHandle& nh) :
-    vo_(NULL),
-    frame_count_(0),
-    img_noise_sigma_(vk::getParam<double>("svo/dataset_noise_sigma", 0.0))
-{
-  // Create Camera
-  if(!vk::camera_loader::loadFromRosNs("svo", cam_))
-    throw std::runtime_error("Camera model not correctly specified.");
-
-  // create pose tracefile
-  std::string trace_est_name(Config::traceDir() + "/traj_estimate.txt");
-  trace_est_pose_.open(trace_est_name.c_str());
-  if(trace_est_pose_.fail())
-    throw std::runtime_error("Could not create tracefile. Does folder exist?");
-
-  // Initialize VO
-  vo_ = new svo::FrameHandlerMono(cam_);
-  vo_->start();
-}
-
-BenchmarkNode::~BenchmarkNode()
-{
-  delete vo_;
-  delete cam_;
-}
-
-void BenchmarkNode::tracePose(const SE3& T_w_f, const double timestamp)
-{
-  Quaterniond q(T_w_f.unit_quaternion());
-  Vector3d p(T_w_f.translation());
-  trace_est_pose_.precision(15);
-  trace_est_pose_.setf(std::ios::fixed, std::ios::floatfield );
-  trace_est_pose_ << timestamp << " ";
-  trace_est_pose_.precision(6);
-  trace_est_pose_ << p.x() << " " << p.y() << " " << p.z() << " "
-                  << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << std::endl;
-}
-
-void BenchmarkNode::tracePoseError(const SE3& T_f_gt, const double timestamp)
-{
-  Vector3d et(T_f_gt.translation()); // translation error
-  trace_trans_error_.precision(15);
-  trace_trans_error_.setf(std::ios::fixed, std::ios::floatfield );
-  trace_trans_error_ << timestamp << " ";
-  trace_trans_error_.precision(6);
-  trace_trans_error_ << et.x() << " " << et.y() << " " << et.z() << " " << std::endl;
-  Vector3d er(vk::dcm2rpy(T_f_gt.rotation_matrix())); // rotation error in roll-pitch-yaw
-  trace_rot_error_.precision(15);
-  trace_rot_error_.setf(std::ios::fixed, std::ios::floatfield );
-  trace_rot_error_ << timestamp << " ";
-  trace_rot_error_.precision(6);
-  trace_rot_error_ << er.x() << " " << er.y() << " " << er.z() << " " << std::endl;
-}
-
-void BenchmarkNode::traceDepthError(const FramePtr& frame, const cv::Mat& depthmap)
-{
-  trace_depth_error_.precision(6);
-  std::for_each(frame->fts_.begin(), frame->fts_.end(), [&](Feature* ftr){
-    if(ftr->point != NULL)
-    {
-      double depth_estimated = (ftr->point->pos_-frame->pos()).norm();
-      double depth_true = depthmap.at<float>((int) ftr->px[1], (int) ftr->px[0]);
-      trace_depth_error_ << frame->id_ << " "
-                         << depth_estimated-depth_true << std::endl;
-    }
-  });
-}
-
-void BenchmarkNode::runBenchmark(const std::string& dataset_dir)
-{
-  // create image reader and load dataset
-  std::string filename_benchmark(dataset_dir + "/images.txt");
-  vk::FileReader<FileType::DatasetImg> dataset_reader(filename_benchmark);
-  dataset_reader.skipComments();
-  if(!dataset_reader.next()) {
-    SVO_ERROR_STREAM("Failed opening dataset: "<<filename_benchmark);
-    return;
-  }
-  std::vector<FileType::DatasetImg> dataset;
-  dataset_reader.readAllEntries(dataset);
-
-  // process dataset
-  int first_frame = vk::getParam<int>("svo/dataset_first_frame", 0);
-  for(auto it = dataset.begin(); it != dataset.end() && ros::ok(); ++it, ++frame_count_)
-  {
-    if(frame_count_ < first_frame)
-      continue;
-    std::string img_filename(dataset_dir + "/" + it->image_name_);
-    cv::Mat img(cv::imread(img_filename, 0));
-    if(img.empty()) {
-      SVO_ERROR_STREAM("Reading image "<<img_filename<<" failed.");
-      return;
-    }
-    vo_->addImage(img, it->timestamp_);
-    visualizer_.publishMinimal(img, vo_->lastFrame(), *vo_, it->timestamp_);
-    visualizer_.visualizeMarkers(vo_->lastFrame(), vo_->coreKeyframes(), vo_->map());
-    if(vo_->stage() == svo::FrameHandlerMono::STAGE_DEFAULT_FRAME)
-      tracePose(vo_->lastFrame()->T_f_w_.inverse(), it->timestamp_);
-  }
-}
-
-void BenchmarkNode::addNoiseToImage(cv::Mat& img, double sigma)
-{
-  uint8_t* p = (uint8_t*) img.data;
-  uint8_t* p_end = img.ptr<uint8_t>(img.rows, img.cols);
-  while(p != p_end)
-  {
-    int val = *p + vk::Sample::gaussian(sigma) + 0.5;
-    *p = std::max(std::min(val, 255), 0);
-    ++p;
-  }
-}
-
-void BenchmarkNode::runBlenderBenchmark(const std::string& dataset_dir)
-{
-  // create image reader and load dataset
-  std::string filename_benchmark(dataset_dir + "/trajectory.txt");
-  vk::FileReader<vk::blender_utils::file_format::ImageNameAndPose> dataset_reader(filename_benchmark);
-  dataset_reader.skipComments();
-  if(!dataset_reader.next()) {
-    SVO_ERROR_STREAM("Failed opening dataset: "<<filename_benchmark);
-    return;
-  }
-  std::vector<vk::blender_utils::file_format::ImageNameAndPose> dataset;
-  dataset_reader.readAllEntries(dataset);
-
-  // create tracefiles
-  trace_trans_error_.open(Config::traceDir() + "/translation_error.txt");
-  trace_rot_error_.open(Config::traceDir() + "/orientation_error.txt");
-  trace_depth_error_.open(Config::traceDir() + "/depth_error.txt");
-  if(trace_trans_error_.fail() || trace_rot_error_.fail() || trace_depth_error_.fail())
-    throw std::runtime_error("Could not create tracefile. Does folder exist?");
-
-  // process dataset
-  for(auto it = dataset.begin(); it != dataset.end() && ros::ok(); ++it, ++frame_count_)
-  {
-    // Read image, ground-truth depth-map and ground-truth pose
-    std::string img_filename(dataset_dir + "/img/" + it->image_name_ + "_0.png");
-    cv::Mat img(cv::imread(img_filename, 0));
-    if(img.empty()) {
-      SVO_ERROR_STREAM("Reading image "<<img_filename<<" failed.");
-      return;
-    }
-    if(img_noise_sigma_ > 0)
-      addNoiseToImage(img, img_noise_sigma_);
-    cv::Mat depthmap;
-    vk::blender_utils::loadBlenderDepthmap(
-        dataset_dir+"/depth/"+it->image_name_+"_0.depth", *cam_, depthmap);
-    Sophus::SE3 T_w_gt(it->q_, it->t_);
-
-    // Set reference frame with depth
-    if(frame_count_ == 0)
-    {
-      // set reference frame at ground-truth pose
-      FramePtr frame_ref(new Frame(cam_, img, it->timestamp_));
-      frame_ref->T_f_w_ = T_w_gt.inverse();
-
-      // extract features, generate features with 3D points
-      svo::feature_detection::FastDetector detector(
-          cam_->width(), cam_->height(), svo::Config::gridSize(), svo::Config::nPyrLevels());
-      detector.detect(frame_ref.get(), frame_ref->img_pyr_, svo::Config::triangMinCornerScore(), frame_ref->fts_);
-      std::for_each(frame_ref->fts_.begin(), frame_ref->fts_.end(), [&](Feature* ftr) {
-        Eigen::Vector3d pt_pos_cur = ftr->f*depthmap.at<float>(ftr->px[1], ftr->px[0]);
-        Eigen::Vector3d pt_pos_world = frame_ref->T_f_w_.inverse()*pt_pos_cur;
-        svo::Point* point = new svo::Point(pt_pos_world, ftr);
-        ftr->point = point;
-      });
-      SVO_INFO_STREAM("Added "<<frame_ref->nObs()<<" 3d pts to the reference frame.");
-      vo_->setFirstFrame(frame_ref);
-      SVO_INFO_STREAM("Set reference frame.");
-    }
-    else
-    {
-      SVO_DEBUG_STREAM("Processing image "<<it->image_name_<<".");
-      vo_->addImage(img, it->timestamp_);
-      visualizer_.publishMinimal(img, vo_->lastFrame(), *vo_, it->timestamp_);
-      visualizer_.visualizeMarkers(vo_->lastFrame(), vo_->coreKeyframes(), vo_->map());
-    }
-
-    if(vo_->stage() != svo::FrameHandlerMono::STAGE_DEFAULT_FRAME)
-    {
-      SVO_ERROR_STREAM("SVO failed before entire dataset could be processed.");
-      break;
-    }
-
-    // Compute pose error and trace to file
-    Sophus::SE3 T_f_gt(vo_->lastFrame()->T_f_w_*T_w_gt);
-    tracePoseError(T_f_gt, it->timestamp_);
-    tracePose(vo_->lastFrame()->T_f_w_.inverse(), it->timestamp_);
-    traceDepthError(vo_->lastFrame(), depthmap);
-  }
-}
-
-} // namespace svo
-
-int main(int argc, char** argv)
-{
-  ros::init(argc, argv, "svo");
-  ros::NodeHandle nh;
-  svo::BenchmarkNode benchmark(nh);
-  std::string benchmark_dir(vk::getParam<std::string>("svo/dataset_directory"));
-  if(vk::getParam<bool>("svo/dataset_is_blender", false))
-    benchmark.runBlenderBenchmark(benchmark_dir);
-  else
-    benchmark.runBenchmark(benchmark_dir);
-  printf("BenchmarkNode finished.\n");
-  return 0;
-}
@@ -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>
@@ -1,4 +1,4 @@
-SET(PROJECT_NAME svo_ros)
+SET(PROJECT_NAME vio_svo)
 PROJECT(${PROJECT_NAME})
 CMAKE_MINIMUM_REQUIRED (VERSION 2.8.3)
 SET(CMAKE_BUILD_TYPE Release) # Release, RelWithDebInfo
@@ -1,15 +1,16 @@
 <launch>

-  <node pkg="svo_ros" type="vo" name="svo" clear_params="true" output="screen">
+  <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 svo_ros)/param/camera_pinhole.yaml" />
+    <rosparam file="$(find vio_svo)/param/camera_pinhole.yaml" />

    <!-- Default parameter settings: choose between vo_fast and vo_accurate -->
-    <rosparam file="$(find svo_ros)/param/px30.yaml" />
+    <rosparam file="$(find vio_svo)/param/px30.yaml" />
    
    <!-- Initial camera orientation, make it point downwards -->
    <param name="init_rx" value="0.00" />
@@ -1,15 +1,15 @@
 <launch>

-  <node pkg="svo_ros" type="vo" name="svo" clear_params="true" output="screen">
+  <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" />

    <!-- Camera calibration file -->
-    <rosparam file="$(find svo_ros)/param/camera_atan.yaml" />
+    <rosparam file="$(find vio_svo)/param/camera_atan.yaml" />

    <!-- Default parameter settings: choose between vo_fast and vo_accurate -->
-    <rosparam file="$(find svo_ros)/param/vo_fast.yaml" />
+    <rosparam file="$(find vio_svo)/param/vo_fast.yaml" />

    <!-- Initial camera orientation, make it point downwards -->
    <param name="init_rx" value="3.14" />
@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 <package>
-  <name>svo_ros</name>
+  <name>vio_svo</name>
  <version>0.1.0</version>
  <description>
    Monocular Visual Odometry - ROS Nodes
@@ -14,7 +14,7 @@
 // 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 <svo_ros/visualizer.h>
+#include <vio_svo/visualizer.h>
 #include <svo/frame_handler_mono.h>
 #include <svo/frame.h>
 #include <svo/point.h>
@@ -17,7 +17,7 @@
 #include <string>
 #include <svo/frame_handler_mono.h>
 #include <svo/map.h>
-#include <svo_ros/visualizer.h>
+#include <vio_svo/visualizer.h>
 #include <vikit/params_helper.h>
 #include <sensor_msgs/Image.h>
 #include <sensor_msgs/Imu.h>
@@ -44,40 +44,45 @@ public:
      vk::AbstractCamera* cam_;
      FILE* logtime;
      bool quit_;
+      boost::thread* vio_th;
      VoNode();
      ~VoNode();
      void imgCb(const sensor_msgs::ImageConstPtr& msg);
+      void imuCb(const sensor_msgs::ImuPtr& imu);
+      void vio();
 };

 VoNode::VoNode() :
      vo_(NULL),
-      publish_markers_(vk::getParam<bool>("svo/publish_markers", true)),
+      publish_markers_(vk::getParam<bool>("vio/publish_markers", true)),
      cam_(NULL),
      quit_(false)
 {
      // Create Camera
-      if(!vk::camera_loader::loadFromRosNs("svo", cam_))
+      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>("svo/init_rx", 0.0),
-                               vk::getParam<double>("svo/init_ry", 0.0),
-                               vk::getParam<double>("svo/init_rz", 0.0))),
-          Eigen::Vector3d(vk::getParam<double>("svo/init_tx", 0.0),
-                          vk::getParam<double>("svo/init_ty", 0.0),
-                          vk::getParam<double>("svo/init_tz", 0.0)));
-
+          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_);
      vo_->start();
      logtime = fopen((std::string(getenv("HOME"))+"/Project/time.txt").c_str(), "w+");
+    vio_th=new boost::thread(&VoNode::vio,this);

 }

 VoNode::~VoNode()
 {
+    quit_=true;
    fclose(logtime);
+    vio_th->join();
+    delete vio_th;
    delete logtime;
    delete vo_;
    delete cam_;
@@ -85,44 +90,53 @@ VoNode::~VoNode()

 void VoNode::imgCb(const sensor_msgs::ImageConstPtr& msg)
 {
-    auto start=std::chrono::steady_clock::now();
      try {
-          cv::Mat img;
-          img = cv_bridge::toCvShare(msg, "mono8")->image;
+          ROS_INFO("image back");
+          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) {
+    ROS_INFO("imu back");
+}
+void VoNode::vio(){
+    ros::NodeHandle nh;
+    std::cout << "create vo_node" << std::endl;
+    // 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"), 5, &svo::VoNode::imgCb, this);
+    // start processing callbacks
+    while(ros::ok() && !quit_)
+    {
+        ros::spinOnce();
+        // TODO check when last image was processed. when too long ago. publish warning that no msgs are received!
+    }
+
+}
 } // namespace svo

 int main(int argc, char **argv)
 {
-  ros::init(argc, argv, "svo");
-  ros::NodeHandle nh;
-  std::cout << "create vo_node" << std::endl;
+  ros::init(argc, argv, "vio");
  svo::VoNode vo_node;
-
-  // subscribe to cam msgs
-  image_transport::ImageTransport it(nh);
-  image_transport::Subscriber it_sub = it.subscribe(vk::getParam<std::string>("svo/cam_topic", "camera/image_raw"), 5, &svo::VoNode::imgCb, &vo_node);
-
+  ros::NodeHandle nh;
+  ros::Subscriber imu_sub=nh.subscribe(vk::getParam<std::string>("vio/imu_topic", "imu/raw"),10,&svo::VoNode::imuCb, &vo_node);
  // start processing callbacks
  while(ros::ok() && !vo_node.quit_)
  {
-    ros::spinOnce();
-    // TODO check when last image was processed. when too long ago. publish warning that no msgs are received!
+      ros::spinOnce();
+      // TODO check when last image was processed. when too long ago. publish warning that no msgs are received!
  }
-
  printf("SVO terminated.\n");
  return 0;
 }