Merge pull request daniilidis-group#12 from daniilidis-group/ros_node

Initial ROS Node Support

.gitignore

@@ -0,0 +1 @@
+euroc/*.bag

CMakeLists.txt

@@ -12,7 +12,7 @@ SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -std=c++14 -DEIGEN_NO_DEBUG")
 IF(DEFINED ENV{ARM_ARCHITECTURE})
   SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS} -mfpu=neon -march=armv7-a")
 ELSE()
-  SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS} -msse4.2 -msse -msse2")
+  SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS} -msse -msse4.2 -msse2")
 ENDIF()
 SET(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS} -std=c++14 -g")
 
@@ -97,6 +97,9 @@ target_link_libraries(msckf_mono
 add_executable(asl_msckf datasets/asl_msckf.cpp datasets/asl_readers.cpp)
 target_link_libraries(asl_msckf msckf_mono ${LINK_LIBS})
 
+add_executable(asl_msckf_no_ground_truth datasets/asl_msckf_no_ground_truth.cpp datasets/asl_readers.cpp)
+target_link_libraries(asl_msckf_no_ground_truth msckf_mono ${LINK_LIBS})
+
 add_executable(msckf_mono_node nodes/msckf_mono_node.cpp)
 target_link_libraries(msckf_mono_node msckf_mono ${LINK_LIBS})
 

README.md

@@ -5,10 +5,24 @@ Monocular MSCKF with ROS Support
 - ROS Kinetic with Boost, OpenCV and Eigen
 - https://github.com/uzh-rpg/fast build and install according to their instructions
 
-# Euroc Dataset
-Build this project inside of a ROS workspace
+# Euroc Dataset -- ROS Bag
+Download MH_03_medium.bag from into the euroc folder in this repository.
 
-Download one (or more) of the datasets from https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets
+```
+wget http://robotics.ethz.ch/~asl-datasets/ijrr_euroc_mav_dataset/machine_hall/MH_03_medium/MH_03_medium.bag
+mv MH_03_medium.bag <path_to_msckf_mono>/euroc/.
+```
+
+Now run the MSCKF on this sequence
+```
+roslaunch msckf_mono euroc.launch
+```
+
+RViz will come up by default showing the odometry and image with tracks.
+
+
+# Euroc Dataset -- ASL Format
+Download one (or more) of the datasets from https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets in the ASL dataset format
 Place
 ```
 %YAML:1.0
@@ -24,7 +38,7 @@ roslaunch msckf_mono asl_mskcf.launch data_set_path:=<directory of mav0 inside o
 
 RViz will come up by default and display the image with tracks on the left and the generated path and map on the right.
 
-![Machine Hall 03 Medium](https://github.com/daniilidis-group/msckf_mono/raw/master/EurocMH03.png)
+![Machine Hall 03 Medium](https://github.com/daniilidis-group/msckf_mono/raw/master/euroc/MH03.png)
 
 The two paths shown, green is ground truth and red is from the MSCKF.
 
@@ -39,6 +53,6 @@ We have run this on platforms ranging from the odroid to a modern laptop, so hop
 - The core MSCKF was used in http://openaccess.thecvf.com/content_cvpr_2017/papers/Zhu_Event-Based_Visual_Inertial_CVPR_2017_paper.pdf
 
 # TODO
-- ROS Node and Nodelet
+- ROS Nodelet
 - Remove OpenCV from opening YAML files
 - PennCOSYVIO Dataset support

datasets/asl_msckf.cpp

@@ -64,8 +64,12 @@ int main(int argc, char** argv)
   camera.c_v = K.at<float>(1,2);
 
   camera.q_CI = cam0->get_q_BS();
+  const auto q_CI = camera.q_CI;
   camera.p_C_I = cam0->get_p_BS();
 
+  ROS_INFO_STREAM("Camera\n- q_CI " << q_CI.x() << "," << q_CI.y() << "," << q_CI.z() << "," << q_CI.w() << "\n" <<
+                  "- p_C_I " << camera.p_C_I.transpose());
+
   float feature_cov;
   nh.param<float>("feature_covariance", feature_cov, 7);