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)