diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index e43c9df..f9f08c5 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -144,8 +144,6 @@ struct Feature { Eigen::aligned_allocator< std::pair > > observations; - // NxN Patch of Anchor Image - std::vector patch; // 3d postion of the feature in the world frame. Eigen::Vector3d position; diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index c497d0f..8077acf 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -14,14 +14,10 @@ #include #include #include - #include -#include -#include #include #include -#include #include #include #include @@ -31,11 +27,6 @@ #include "feature.hpp" #include -#include -#include -#include -#include - namespace msckf_vio { /* * @brief MsckfVio Implements the algorithm in @@ -112,12 +103,6 @@ class MsckfVio { */ void featureCallback(const CameraMeasurementConstPtr& msg); - - void imageCallback ( - const sensor_msgs::ImageConstPtr& cam0_img, - const sensor_msgs::ImageConstPtr& cam1_img, - const CameraMeasurementConstPtr& feature_msg); - /* * @brief publish Publish the results of VIO. * @param time The time stamp of output msgs. @@ -141,11 +126,6 @@ class MsckfVio { bool resetCallback(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res); - void manageMovingWindow( - const cv_bridge::CvImageConstPtr& cam0_image_ptr, - const cv_bridge::CvImageConstPtr& cam1_image_ptr, - const CameraMeasurementConstPtr& feature_msg); - // Filter related functions // Propogate the state void batchImuProcessing( @@ -199,15 +179,6 @@ class MsckfVio { // transfer delay between IMU and Image messages. std::vector imu_msg_buffer; - // Moving Window buffer - std::map, - Eigen::aligned_allocator< - std::pair > > cam0_moving_window; - - std::map, - Eigen::aligned_allocator< - std::pair > > cam1_moving_window; - // Indicate if the gravity vector is set. bool is_gravity_set; @@ -235,18 +206,12 @@ class MsckfVio { // Subscribers and publishers ros::Subscriber imu_sub; + ros::Subscriber feature_sub; ros::Publisher odom_pub; ros::Publisher feature_pub; tf::TransformBroadcaster tf_pub; ros::ServiceServer reset_srv; - - message_filters::Subscriber cam0_img_sub; - message_filters::Subscriber cam1_img_sub; - message_filters::Subscriber feature_sub; - - message_filters::TimeSynchronizer image_sub; - // Frame id std::string fixed_frame_id; std::string child_frame_id; diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index c045040..d9c1b8d 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -53,7 +53,6 @@ map MsckfVio::chi_squared_test_table; MsckfVio::MsckfVio(ros::NodeHandle& pnh): is_gravity_set(false), is_first_img(true), - image_sub(10), nh(pnh) { return; } @@ -187,16 +186,8 @@ bool MsckfVio::createRosIO() { imu_sub = nh.subscribe("imu", 100, &MsckfVio::imuCallback, this); - //feature_sub = nh.subscribe("features", 40, - // &MsckfVio::featureCallback, this); - - cam0_img_sub.subscribe(nh, "cam0_image", 10); - cam1_img_sub.subscribe(nh, "cam1_image", 10); - feature_sub.subscribe(nh, "features", 10); - - image_sub.connectInput(cam0_img_sub, cam1_img_sub, feature_sub); - image_sub.registerCallback(&MsckfVio::imageCallback, this); - + feature_sub = nh.subscribe("features", 40, + &MsckfVio::featureCallback, this); mocap_odom_sub = nh.subscribe("mocap_odom", 10, &MsckfVio::mocapOdomCallback, this); @@ -254,119 +245,6 @@ void MsckfVio::imuCallback( return; } -void MsckfVio::imageCallback ( - const sensor_msgs::ImageConstPtr& cam0_img, - 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; - - // Get the current image. - cv_bridge::CvImageConstPtr cam0_image_ptr = cv_bridge::toCvShare(cam0_img, - sensor_msgs::image_encodings::MONO8); - cv_bridge::CvImageConstPtr cam1_img_ptr = cv_bridge::toCvShare(cam1_img, - sensor_msgs::image_encodings::MONO8); - - // Start the system if the first image is received. - // The frame where the first image is received will be - // the origin. - if (is_first_img) { - is_first_img = false; - state_server.imu_state.time = feature_msg->header.stamp.toSec(); - } - - static double max_processing_time = 0.0; - static int critical_time_cntr = 0; - double processing_start_time = ros::Time::now().toSec(); - - // Propogate the IMU state. - // that are received before the image feature_msg. - ros::Time start_time = ros::Time::now(); - batchImuProcessing(feature_msg->header.stamp.toSec()); - double imu_processing_time = ( - ros::Time::now()-start_time).toSec(); - - // Augment the state vector. - start_time = ros::Time::now(); - stateAugmentation(feature_msg->header.stamp.toSec()); - double state_augmentation_time = ( - ros::Time::now()-start_time).toSec(); - - // Add new observations for existing features or new - // features in the map server. - start_time = ros::Time::now(); - addFeatureObservations(feature_msg); - double add_observations_time = ( - ros::Time::now()-start_time).toSec(); - - // Add new images to moving window - start_time = ros::Time::now(); - manageMovingWindow(cam0_image_ptr, cam1_img_ptr, feature_msg); - double manage_moving_window_time = ( - ros::Time::now()-start_time).toSec(); - - // Perform measurement update if necessary. - start_time = ros::Time::now(); - removeLostFeatures(); - double remove_lost_features_time = ( - ros::Time::now()-start_time).toSec(); - - start_time = ros::Time::now(); - pruneCamStateBuffer(); - double prune_cam_states_time = ( - ros::Time::now()-start_time).toSec(); - - // Publish the odometry. - start_time = ros::Time::now(); - publish(feature_msg->header.stamp); - double publish_time = ( - ros::Time::now()-start_time).toSec(); - - // Reset the system if necessary. - onlineReset(); - - double processing_end_time = ros::Time::now().toSec(); - double processing_time = - processing_end_time - processing_start_time; - if (processing_time > 1.0/frame_rate) { - ++critical_time_cntr; - //ROS_INFO("\033[1;31mTotal processing time %f/%d...\033[0m", - // processing_time, critical_time_cntr); - //printf("IMU processing time: %f/%f\n", - // imu_processing_time, imu_processing_time/processing_time); - //printf("State augmentation time: %f/%f\n", - // state_augmentation_time, state_augmentation_time/processing_time); - //printf("Add observations time: %f/%f\n", - // add_observations_time, add_observations_time/processing_time); - //printf("Remove lost features time: %f/%f\n", - // remove_lost_features_time, remove_lost_features_time/processing_time); - //printf("Remove camera states time: %f/%f\n", - // prune_cam_states_time, prune_cam_states_time/processing_time); - //printf("Publish time: %f/%f\n", - // publish_time, publish_time/processing_time); - } - - return; -} - -void MsckfVio::manageMovingWindow( - const cv_bridge::CvImageConstPtr& cam0_image_ptr, - 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; - - while(cam0_moving_window.size() > 100) - { - cam1_moving_window.erase(cam1_moving_window.begin()); - cam0_moving_window.erase(cam0_moving_window.begin()); - } -} - void MsckfVio::initializeGravityAndBias() { // Initialize gravity and gyro bias. @@ -412,6 +290,7 @@ bool MsckfVio::resetCallback( ROS_WARN("Start resetting msckf vio..."); // Temporarily shutdown the subscribers to prevent the // state from updating. + feature_sub.shutdown(); imu_sub.shutdown(); // Reset the IMU state. @@ -469,11 +348,10 @@ bool MsckfVio::resetCallback( // Restart the subscribers. imu_sub = nh.subscribe("imu", 100, &MsckfVio::imuCallback, this); + feature_sub = nh.subscribe("features", 40, + &MsckfVio::featureCallback, this); -// feature_sub = nh.subscribe("features", 40, -// &MsckfVio::featureCallback, this); - -// TODO: When can the reset fail? + // TODO: When can the reset fail? res.success = true; ROS_WARN("Resetting msckf vio completed..."); return true; @@ -876,7 +754,6 @@ void MsckfVio::stateAugmentation(const double& time) { return; } - void MsckfVio::addFeatureObservations( const CameraMeasurementConstPtr& msg) { @@ -1421,8 +1298,6 @@ void MsckfVio::pruneCamStateBuffer() { // Remove this camera state in the state vector. state_server.cam_states.erase(cam_id); - cam0_moving_window.erase(cam_id); - cam1_moving_window.erase(cam_id); } return;