diff --git a/include/msckf_vio/cam_state.h b/include/msckf_vio/cam_state.h index 8d528a1..c6d2f7c 100644 --- a/include/msckf_vio/cam_state.h +++ b/include/msckf_vio/cam_state.h @@ -62,6 +62,10 @@ struct CAMState { typedef std::map, Eigen::aligned_allocator< std::pair > > CamStateServer; + +typedef std::map, + Eigen::aligned_allocator< + std::pair > > movingWindow; } // namespace msckf_vio #endif // MSCKF_VIO_CAM_STATE_H diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index e43c9df..0ff2137 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -114,6 +114,19 @@ struct Feature { inline bool checkMotion( const CamStateServer& cam_states) const; + /* + * @brief InitializeAnchor generates the NxN patch around the + * feature in the Anchor image + * @param cam_states: A map containing all recorded images + * currently presented in the camera state vector + * @return the irradiance of the Anchor NxN Patch + * @return True if the Anchor can be estimated + */ + + inline bool initializeAnchor( + const movingWindow& cam0_moving_window); + + /* * @brief InitializePosition Intialize the feature position * based on all current available measurements. @@ -145,7 +158,7 @@ struct Feature { std::pair > > observations; // NxN Patch of Anchor Image - std::vector patch; + std::vector anchorPatch; // 3d postion of the feature in the world frame. Eigen::Vector3d position; @@ -292,6 +305,29 @@ bool Feature::checkMotion( else return false; } +bool Feature::initializeAnchor( + const movingWindow& cam0_moving_window) +{ + + int N = 5; + int n = (int)(N-1)/2; + + auto anchor = observations.begin(); + if(cam0_moving_window.find(anchor->first) == cam0_moving_window.end()) + return false; + + cv::Mat anchorImage = cam0_moving_window.find(anchor->first)->second; + auto u = anchor->second(0)*anchorImage.rows/2 + anchorImage.rows/2; + auto v = anchor->second(1)*anchorImage.cols/2 + anchorImage.cols/2; + int count = 0; + + for(int u_run = (int)u - n; u_run <= (int)u + n; u_run++) + for(int v_run = v - n; v_run <= v + n; v_run++) + anchorPatch.push_back(anchorImage.at(u_run,v_run)); + + return true; +} + bool Feature::initializePosition( const CamStateServer& cam_states) { // Organize camera poses and feature observations properly. diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index c497d0f..7c2dd73 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -106,13 +107,14 @@ class MsckfVio { void imuCallback(const sensor_msgs::ImuConstPtr& msg); /* - * @brief featureCallback - * Callback function for feature measurements. - * @param msg Stereo feature measurements. + * @brief imageCallback + * Callback function for feature measurements + * Triggers measurement update + * @param msg + * Camera 0 Image + * Camera 1 Image + * Stereo feature measurements. */ - void featureCallback(const CameraMeasurementConstPtr& msg); - - void imageCallback ( const sensor_msgs::ImageConstPtr& cam0_img, const sensor_msgs::ImageConstPtr& cam1_img, @@ -200,14 +202,9 @@ class MsckfVio { std::vector imu_msg_buffer; // Moving Window buffer - std::map, - Eigen::aligned_allocator< - std::pair > > cam0_moving_window; + movingWindow cam0_moving_window; + movingWindow cam1_moving_window; - std::map, - Eigen::aligned_allocator< - std::pair > > cam1_moving_window; - // Indicate if the gravity vector is set. bool is_gravity_set; diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index bff5d38..dd5e110 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -187,8 +187,6 @@ 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); @@ -236,7 +234,8 @@ bool MsckfVio::initialize() { } void MsckfVio::imuCallback( - const sensor_msgs::ImuConstPtr& msg) { + const sensor_msgs::ImuConstPtr& msg) +{ // IMU msgs are pushed backed into a buffer instead of // being processed immediately. The IMU msgs are processed @@ -254,7 +253,7 @@ void MsckfVio::imuCallback( return; } -void MsckfVio::imageCallback ( +void MsckfVio::imageCallback( const sensor_msgs::ImageConstPtr& cam0_img, const sensor_msgs::ImageConstPtr& cam1_img, const CameraMeasurementConstPtr& feature_msg) @@ -358,9 +357,8 @@ void MsckfVio::manageMovingWindow( 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) + //TODO handle any massive overflow correctly (should be pruned, before this ever triggers) while(cam0_moving_window.size() > 100) { cam1_moving_window.erase(cam1_moving_window.begin()); @@ -480,88 +478,6 @@ bool MsckfVio::resetCallback( return true; } -void MsckfVio::featureCallback( - const CameraMeasurementConstPtr& msg) { - - // Return if the gravity vector has not been set. - if (!is_gravity_set) return; - - // 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 = 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 msg. - ros::Time start_time = ros::Time::now(); - batchImuProcessing(msg->header.stamp.toSec()); - double imu_processing_time = ( - ros::Time::now()-start_time).toSec(); - - // Augment the state vector. - start_time = ros::Time::now(); - stateAugmentation(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(msg); - double add_observations_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(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::mocapOdomCallback( const nav_msgs::OdometryConstPtr& msg) { static bool first_mocap_odom_msg = true; @@ -1198,6 +1114,12 @@ void MsckfVio::removeLostFeatures() { } } + if(!feature.initializeAnchor(cam0_moving_window)) + { + invalid_feature_ids.push_back(feature.id); + continue; + } + jacobian_row_size += 4*feature.observations.size() - 3; processed_feature_ids.push_back(feature.id); }