diff --git a/CMakeLists.txt b/CMakeLists.txt index 1a77616..d81aed6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 2.8.12) -project(msckf) +project(msckf_vio) add_compile_options(-std=c++11) diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index c045040..bff5d38 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -259,7 +259,6 @@ void MsckfVio::imageCallback ( const sensor_msgs::ImageConstPtr& cam1_img, const CameraMeasurementConstPtr& feature_msg) { - ROS_INFO("In Callback"); // Return if the gravity vector has not been set. if (!is_gravity_set) return; @@ -357,9 +356,11 @@ void MsckfVio::manageMovingWindow( const cv_bridge::CvImageConstPtr& cam1_image_ptr, const CameraMeasurementConstPtr& feature_msg) { - cam0_moving_window[state_server.imu_state.id] = cam0_image_ptr->image; - cam1_moving_window[state_server.imu_state.id] = cam1_image_ptr->image; + cam0_moving_window[state_server.imu_state.id] = cam0_image_ptr->image.clone(); + cam1_moving_window[state_server.imu_state.id] = cam1_image_ptr->image.clone(); + std::cout << cam0_moving_window.begin()->second << std::endl; + //TODO handle massive overflow correctly (should be pruned, before this ever triggers) while(cam0_moving_window.size() > 100) { cam1_moving_window.erase(cam1_moving_window.begin());