complete pipline and test with unity_dataset

vio_svo/CMakeLists.txt

@@ -8,7 +8,7 @@ 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} -mfpu=neon -march=armv7-a")
+  SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}")
 ELSE()
   SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mmmx -msse -msse -msse2 -msse3 -mssse3")
 ENDIF()
@@ -20,8 +20,9 @@ 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 
-  roscpp
+FIND_PACKAGE(catkin REQUIRED COMPONENTS
+        geometry_msgs
+        roscpp
   cmake_modules
   nav_msgs
   std_msgs
@@ -41,11 +42,12 @@ 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
+  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
 )
@@ -70,10 +72,10 @@ LIST(APPEND LINK_LIBS
 
 # Create visualizer library
 ADD_LIBRARY(svo_visualizer src/visualizer.cpp)
-TARGET_LINK_LIBRARIES(svo_visualizer ${LINK_LIBS})
+TARGET_LINK_LIBRARIES(svo_visualizer ${LINK_LIBS} gtsam)
                                  
 # Create Executables                      
 ADD_EXECUTABLE(vo src/vo_node.cpp)
-TARGET_LINK_LIBRARIES(vo svo_visualizer)
+TARGET_LINK_LIBRARIES(vo svo_visualizer ${LINK_LIBS})
 #ADD_EXECUTABLE(benchmark src/benchmark_node.cpp)
 #TARGET_LINK_LIBRARIES(benchmark svo_visualizer)

vio_svo/launch/unity.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>

vio_svo/package.xml

@@ -26,6 +26,7 @@
   <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>
@@ -40,4 +41,5 @@
   <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>

vio_svo/param/unity.yaml

@@ -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

vio_svo/src/vo_node.cpp

@@ -22,6 +22,7 @@
 #include <sensor_msgs/Image.h>
 #include <sensor_msgs/Imu.h>
 #include <std_msgs/String.h>
+#include <geometry_msgs/PoseStamped.h>
 #include <message_filters/synchronizer.h>
 #include <image_transport/image_transport.h>
 #include <boost/thread.hpp>
@@ -49,7 +50,8 @@ public:
       VoNode();
       ~VoNode();
       void imgCb(const sensor_msgs::ImageConstPtr& msg);
-      void imuCb(const sensor_msgs::ImuPtr& imu);
+      //void imuCb(const sensor_msgs::ImuPtr& imu);
+      void imuCb(const geometry_msgs::PoseStamped& imu);
       void imu();
 };
 
@@ -106,15 +108,22 @@ void VoNode::imgCb(const sensor_msgs::ImageConstPtr& msg)
         ROS_ERROR("cv_bridge exception: %s", e.what());
       }
 }
-void VoNode::imuCb(const sensor_msgs::ImuPtr &imu) {
-    float imu_in[7];
-    imu_in[0] = imu->angular_velocity.x;
+void VoNode::imuCb(const geometry_msgs::PoseStamped &imu) {
+    float imu_in[6];
+    /*imu_in[0] = imu->angular_velocity.x;
     imu_in[1] = imu->angular_velocity.y;
     imu_in[2] = imu->angular_velocity.z;
     imu_in[3] = imu->linear_acceleration.x;
     imu_in[4] = imu->linear_acceleration.y;
     imu_in[5] = imu->linear_acceleration.z;
     imu_in[6] = imu->header.stamp.sec;
+     */
+    imu_in[0] = imu.pose.position.z;
+    imu_in[1] = imu.pose.position.x;
+    imu_in[2] = imu.pose.position.y+9.8;
+    imu_in[3] = imu.pose.orientation.z;
+    imu_in[4] = imu.pose.orientation.x;
+    imu_in[5] = imu.pose.orientation.y;
     vo_->imu_integPtr_->update(imu_in);
 }
 void VoNode::imu(){
@@ -137,11 +146,12 @@ 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"),10,&svo::VoNode::imuCb, &vo_node);
+  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");