diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index f9f08c5..e43c9df 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -144,6 +144,8 @@ 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/image_processor.h b/include/msckf_vio/image_processor.h index 1004086..086c5a9 100644 --- a/include/msckf_vio/image_processor.h +++ b/include/msckf_vio/image_processor.h @@ -14,10 +14,6 @@ #include #include -#include -#include -#include - #include #include #include @@ -312,7 +308,7 @@ private: const std::vector& markers, std::vector& refined_vec) { if (raw_vec.size() != markers.size()) { - ROS_WARN("The input size of raw_vec(%i) and markers(%i) does not match...", + ROS_WARN("The input size of raw_vec(%lu) and markers(%lu) does not match...", raw_vec.size(), markers.size()); } for (int i = 0; i < markers.size(); ++i) { @@ -367,11 +363,6 @@ private: boost::shared_ptr prev_features_ptr; boost::shared_ptr curr_features_ptr; - cv::cuda::GpuMat cam0_curr_img; - cv::cuda::GpuMat cam1_curr_img; - cv::cuda::GpuMat cam0_points_gpu; - cv::cuda::GpuMat cam1_points_gpu; - // Number of features after each outlier removal step. int before_tracking; int after_tracking; diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index 8077acf..c497d0f 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -14,10 +14,14 @@ #include #include #include + #include +#include +#include #include #include +#include #include #include #include @@ -27,6 +31,11 @@ #include "feature.hpp" #include +#include +#include +#include +#include + namespace msckf_vio { /* * @brief MsckfVio Implements the algorithm in @@ -103,6 +112,12 @@ 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. @@ -126,6 +141,11 @@ 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( @@ -179,6 +199,15 @@ 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; @@ -206,12 +235,18 @@ 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/include/msckf_vio/utils.h b/include/msckf_vio/utils.h index cf0d39a..97f0dac 100644 --- a/include/msckf_vio/utils.h +++ b/include/msckf_vio/utils.h @@ -11,9 +11,6 @@ #include #include #include -#include -#include -#include #include namespace msckf_vio { @@ -21,10 +18,6 @@ namespace msckf_vio { * @brief utilities for msckf_vio */ namespace utils { - -void download(const cv::cuda::GpuMat& d_mat, std::vector& vec); -void download(const cv::cuda::GpuMat& d_mat, std::vector& vec); - Eigen::Isometry3d getTransformEigen(const ros::NodeHandle &nh, const std::string &field); diff --git a/msg/CameraMeasurement.msg b/msg/CameraMeasurement.msg index 29a69bf..84e448d 100644 --- a/msg/CameraMeasurement.msg +++ b/msg/CameraMeasurement.msg @@ -1,4 +1,4 @@ -std_msgs/Header header +Header header # All features on the current image, # including tracked ones and newly detected ones. FeatureMeasurement[] features diff --git a/src/image_processor.cpp b/src/image_processor.cpp index ba220b1..a5039f3 100644 --- a/src/image_processor.cpp +++ b/src/image_processor.cpp @@ -170,10 +170,6 @@ bool ImageProcessor::loadParameters() { processor_config.ransac_threshold); ROS_INFO("stereo_threshold: %f", processor_config.stereo_threshold); - ROS_INFO("OpenCV Major Version: %d", - CV_MAJOR_VERSION); - ROS_INFO("OpenCV Minor Version: %d", - CV_MINOR_VERSION); ROS_INFO("==========================================="); return true; } @@ -223,9 +219,7 @@ void ImageProcessor::stereoCallback( sensor_msgs::image_encodings::MONO8); // Build the image pyramids once since they're used at multiple places - - // removed due to alternate cuda construct - //createImagePyramids(); + createImagePyramids(); // Detect features in the first frame. if (is_first_img) { @@ -302,7 +296,6 @@ void ImageProcessor::imuCallback( void ImageProcessor::createImagePyramids() { const Mat& curr_cam0_img = cam0_curr_img_ptr->image; - // TODO: build cuda optical flow buildOpticalFlowPyramid( curr_cam0_img, curr_cam0_pyramid_, Size(processor_config.patch_size, processor_config.patch_size), @@ -310,7 +303,6 @@ void ImageProcessor::createImagePyramids() { BORDER_CONSTANT, false); const Mat& curr_cam1_img = cam1_curr_img_ptr->image; - // TODO: build cuda optical flow buildOpticalFlowPyramid( curr_cam1_img, curr_cam1_pyramid_, Size(processor_config.patch_size, processor_config.patch_size), @@ -464,7 +456,6 @@ void ImageProcessor::trackFeatures() { predictFeatureTracking(prev_cam0_points, cam0_R_p_c, cam0_intrinsics, curr_cam0_points); - //TODO: change to GPU calcOpticalFlowPyrLK( prev_cam0_pyramid_, curr_cam0_pyramid_, prev_cam0_points, curr_cam0_points, @@ -634,30 +625,7 @@ void ImageProcessor::stereoMatch( cam1_distortion_model, cam1_distortion_coeffs); } - auto d_pyrLK_sparse = cuda::SparsePyrLKOpticalFlow::create( - Size(processor_config.patch_size, processor_config.patch_size), - processor_config.pyramid_levels, - processor_config.max_iteration, - true); - - cam0_curr_img = cv::cuda::GpuMat(cam0_curr_img_ptr->image); - cam1_curr_img = cv::cuda::GpuMat(cam1_curr_img_ptr->image); - cam0_points_gpu = cv::cuda::GpuMat(cam0_points); - cam1_points_gpu = cv::cuda::GpuMat(cam1_points); - - cv::cuda::GpuMat inlier_markers_gpu; - d_pyrLK_sparse->calc(cam0_curr_img, - cam1_curr_img, - cam0_points_gpu, - cam1_points_gpu, - inlier_markers_gpu, - noArray()); - - utils::download(cam1_points_gpu, cam1_points); - utils::download(inlier_markers_gpu, inlier_markers); - // Track features using LK optical flow method. - /* calcOpticalFlowPyrLK(curr_cam0_pyramid_, curr_cam1_pyramid_, cam0_points, cam1_points, inlier_markers, noArray(), @@ -667,7 +635,7 @@ void ImageProcessor::stereoMatch( processor_config.max_iteration, processor_config.track_precision), cv::OPTFLOW_USE_INITIAL_FLOW); - */ + // Mark those tracked points out of the image region // as untracked. for (int i = 0; i < cam1_points.size(); ++i) { @@ -1027,7 +995,7 @@ void ImageProcessor::twoPointRansac( // Check the size of input point size. if (pts1.size() != pts2.size()) - ROS_ERROR("Sets of different size (%i and %i) are used...", + ROS_ERROR("Sets of different size (%lu and %lu) are used...", pts1.size(), pts2.size()); double norm_pixel_unit = 2.0 / (intrinsics[0]+intrinsics[1]); diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index d9c1b8d..c045040 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -53,6 +53,7 @@ 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; } @@ -186,8 +187,16 @@ bool MsckfVio::createRosIO() { 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); + + 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); + mocap_odom_sub = nh.subscribe("mocap_odom", 10, &MsckfVio::mocapOdomCallback, this); @@ -245,6 +254,119 @@ 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. @@ -290,7 +412,6 @@ 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. @@ -348,10 +469,11 @@ bool MsckfVio::resetCallback( // Restart the subscribers. imu_sub = nh.subscribe("imu", 100, &MsckfVio::imuCallback, this); - feature_sub = nh.subscribe("features", 40, - &MsckfVio::featureCallback, this); - // TODO: When can the reset fail? +// feature_sub = nh.subscribe("features", 40, +// &MsckfVio::featureCallback, this); + +// TODO: When can the reset fail? res.success = true; ROS_WARN("Resetting msckf vio completed..."); return true; @@ -754,6 +876,7 @@ void MsckfVio::stateAugmentation(const double& time) { return; } + void MsckfVio::addFeatureObservations( const CameraMeasurementConstPtr& msg) { @@ -1298,6 +1421,8 @@ 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; diff --git a/src/utils.cpp b/src/utils.cpp index 830d84f..2af7278 100644 --- a/src/utils.cpp +++ b/src/utils.cpp @@ -11,20 +11,6 @@ namespace msckf_vio { namespace utils { -void download(const cv::cuda::GpuMat& d_mat, std::vector& vec) -{ - vec.resize(d_mat.cols); - cv::Mat mat(1, d_mat.cols, CV_32FC2, (void*)&vec[0]); - d_mat.download(mat); -} - -void download(const cv::cuda::GpuMat& d_mat, std::vector& vec) -{ - vec.resize(d_mat.cols); - cv::Mat mat(1, d_mat.cols, CV_8UC1, (void*)&vec[0]); - d_mat.download(mat); -} - Eigen::Isometry3d getTransformEigen(const ros::NodeHandle &nh, const std::string &field) { Eigen::Isometry3d T;